/***************************************************************************
 创建者: 华磊
 开始时间: 2017.6.28
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 *  @file trajectoryserver.cpp
 *  功能:
 *　提供轨迹规划服务
 *
 *                                                                         *
 ***************************************************************************/
#include "trajectoryserver.h"
#include "timetool.h"
#include <sys/syscall.h>
#include "parametermanagement.h"
#ifndef MOTION_SERVER_UNIT_TEST
#include "robotcontrol.h"
#endif
#include <QSettings>
#include <QVariant>
#include "cu1sensoradapter.h"
#include "qrcodesensoradapter.h"
#include "magnetictrack.h"
#include "belttrackmanager.h"
#include "couplecontrolmanagement.h"
#include "robotio.h"
//#include "wavemotion.h"
#include "gantrycontrol.h"
#include "zerocalculate.h"
#include "carvingmodule.h"

#define D_TRIG_DEBUG 0
#define D_STOP_TIME_MAX 8
#define D_RECORD_ADIN 1
#define D_STOP_SPEED_RATIO 0.00001
extern RobotControl* robCtl;
#ifdef D_USE_ROS
#include "rosapi.h"
//extern RosApi *m_rosApi;
#endif

using namespace std;
double g_waveMonitorData[D_WAVE_AXIS_SIZE];
int min_v2[3]={-10000,-20000,-50000};
int max_v2[3]={10000,20000,50000};
int timer_id2[20];
enum E_TIME2
{
    E_TIME2_C1,
    E_TIME2_C2,
    E_TIME2_C3,
    E_TIME2_C4,
    E_TIME2_C5,
    E_TIME2_C6,
    E_TIME2_C7,
    E_TIME2_C8,
    E_TIME2_C9,
    E_TIME2_C10,
    E_TIME2_C11

};


int min_v3[3]={-10000,-20000,-50000};
int max_v3[3]={10000,20000,50000};
int timer_id3[20];
enum E_TIME3{
    E_TIME3_C1,
    E_TIME3_C2,
    E_TIME3_C3,
    E_TIME3_C4,
    E_TIME3_C5,
    E_TIME3_C6,
    E_TIME3_C7,
    E_TIME3_C8,
    E_TIME3_C9,
    E_TIME3_C10,
    E_TIME3_C11

};

//#define D_MAGANETIC_ALIAS_FRONT 10
//#define D_MAGANETIC_ALIAS_BACK 11
//#define D_RFID_ALIAS_FRONT 15
//#define D_RFID_ALIAS_BACK 16
#define D_LINE_DEBUG 1
#define D_COMMON_LOOP_PERIOD 30000 //us


TrajectoryServer::TrajectoryServer(RobotCommonParameter * roboParameterIn,
                                   MotionMessage *motionMessageIn, RosApi* rosApiIn, RegData *regDataIn,
                                   RobotIo *ioControllerIn, int recordWareFlagIn)
{
    ioController=ioControllerIn;
    regData=regDataIn;
    isInitialOkStatus=true;
    laserLoseRescueFlag=0;
    recordWareFlag=recordWareFlagIn;
//    currentFrame_Feedback.resize(1);
//    motionModel=ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS;
    motionModel=ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK;
    motionMessage=motionMessageIn;
    roboParameter=roboParameterIn;
    robotId=roboParameter->robotId;
    m_rosApi=rosApiIn;
//    t1RatioLimit=roboParameter->t1RatioLimit;
    targetWaitTime=roboParameter->targetWaitTime;
    lineMaxSpeed=roboParameter->lineMaxSpeed;
    lineMaxAcceleration=roboParameter->lineMaxAcceleration;
    //deltaTime=roboParameter->deltaTime;
    deltaTime=robCtl->getCycle()/1000000.0;//dc time, 单位秒
    roboParameter->deltaTime=deltaTime;
    allowSingleAxisJogFlag=false;
    isMoveToPoint=false;
    allowServonCheck=1;
    allowServonCheckDifference=1;
    vehicleType=E_VEHICLE_TYPE_TWO_DIFFERENTIAL_DRIVE;
    debugFlag=0;
    updateTrajStage=0;
    calibrateCheckFinished=false;
    isMotorCommandValid=true;
    isCoordinatePlanInitialed=false;
    motionTrigStart=false;
    zeroMovingStage=0;

    axisLogicalTypeVector.clear();
    for(int i=0;i<roboParameter->jointAttributeVector.size();i++)
    {
        axisLogicalTypeVector.append(roboParameter->jointAttributeVector[i].axisLogicalType);
    }

    ParameterManagement paraManage(robotId);
    paraManage.getServonParameter(allowServonCheck,allowServonCheckDifference);

    coordinateManager=new CoordinateManager(roboParameter,motionMessage);
    zeroCalculate=new ZeroCalculate(coordinateManager,roboParameter);

    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString configFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/dragConfig.xml";
    dragControl= new DragControl(configFile,this,roboParameter);
    if(false==dragControl->isInitialOk())
    {
        isInitialOkStatus=false;
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5079,robotId);
    }

    tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    configFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/vehicle.ini";
    magneticTrack= new MagneticTrack(configFile,motionMessageIn);
    if(false==magneticTrack->isInitialOk())
    {
        isInitialOkStatus=false;
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5027,robotId);
    }
    if(1!=readVehicleType(configFile,vehicleType))
    {
        isInitialOkStatus=false;
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5045,robotId);
    }

    vehicleJogControl= new VehicleJogControl(configFile,motionMessageIn,robotId,deltaTime);

    QString configFile_virtualLine = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/virtualLineConfig.xml";
    virtualLineControl=new VirtualLineControl(configFile_virtualLine,robotId,roboParameter,
                                              motionMessageIn,magneticTrack,vehicleType,ioControllerIn);
    QString configFile2 = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/odometer.ini";
    QString configFile3 = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/positionConfig.ini";
    vehiclePositionManage=new VehiclePositionManage(robotId,motionMessageIn,configFile2,configFile3,deltaTime);

    configFile=D_TCRCONTROLLER_DATA_PATH;
    configFile+="/globalConfig/cu1Sensor/cu1SensorConfig.xml";
    cu1Sensor=new Cu1SensorAdapter(configFile);
    if(false==cu1Sensor->isInitialOk())
    {
        isInitialOkStatus=false;
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5047,robotId);
    }

    QString beltConfigFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/beltTrackConfig.xml";
    beltTrackManager=new BeltTrackManager(beltConfigFile,coordinateManager,regData,motionMessage,robotId,deltaTime,ioControllerIn);


#ifdef D_USE_ROS
    configFile=D_TCRCONTROLLER_DATA_PATH;
    configFile+="/globalConfig/qrcodeSensor/qrcodeSensorConfig.xml";
    qrcodeSensorAdapter=new QrcodeSensorAdapter(configFile);
    if(false==qrcodeSensorAdapter->isInitialOk())
    {
        isInitialOkStatus=false;
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5050,robotId);
    }
#endif


    //---------初始化MC线程锁--------
    pthread_mutexattr_init(&trajAttr);
    pthread_mutexattr_setprotocol(&trajAttr,PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&trajMutex,&trajAttr);

    pthread_mutexattr_init(&getAttr);
    pthread_mutexattr_setprotocol(&getAttr,PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&getMutex,&getAttr);

    commandQueue.clear();

    axisGroup=new AxisGroup(roboParameter->robotId, roboParameter,roboParameter->jointAttributeVector,
                            roboParameter->coordinateAttribute,0
                            ,roboParameter->deltaTime,roboParameter->robotType,motionMessage,coordinateManager) ;

    tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    configFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/coupleControlConfig.xml";
    coupleManagement = new CoupleControlManagement(configFile,motionMessageIn,robotId,axisGroup);

    configFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/config/gantryControlConfig.xml";
    gantryControl =new GantryControl(configFile,motionMessageIn,robotId,axisGroup);

//    waveMotionControl=new WaveMotion(deltaTime);

    isInCarvingTp=false;
    carvingModule=new CarvingModule(roboParameter,coordinateManager,motionMessageIn);


    safeSpeedRatio=1;
    controlSpeedRatio=1;
    pidSpeedRatio=1;
    adjustSpeedRatio=1;
    mainStatusRatio=0;

    timeAxisScale=0.0;
    axisAccelerationRatio=0;

    basicStatus=ENUM_TRAJECTORY_STOPPED;

    currentLine=ENUM_LINE_INITIAL;
    isCurrentLineFinished=-1;
    currentLineRemainingTime=-1;

    jointsNum=roboParameter->jointNum;
    innerJointsNum=roboParameter->armControlAxisNumber;
    externJointsNum=roboParameter->externalNum;//not used
    agvAxisNum=roboParameter->agvAxisNum;
    virtualAxisNum=roboParameter->virtualAxisNum;
    robotType=roboParameter->robotType;

    timeAxisParameter.slowDownRatio=0.1;
    timeAxisParameter.startAcceleration=0.2;
    timeAxisParameter.startDuration=3;
    timeAxisParameter.stopAcceleration=0.6;
    timeAxisParameter.stopDuration=5;

    timeAxisStatus=ENUM_TIME_AXIS_NORMAL;
    oldTimeAxisRatio=0.0;
    newTimeAxisRatio=0.0;

//    currentJointPoint_Command.configInt.resize(6);
//    currentJointPoint_Command.positionValue.resize(jointsNum);
//    currentJointPoint_Command.extPositionValue.resize( 0 );
//    currentCoordinatePoint_Command.configInt.resize(6);
//    currentCoordinatePoint_Command.positionValue.resize(jointsNum);
//    currentCoordinatePoint_Command.extPositionValue.resize( 0 );
    currentJointPoint_Feedback.configInt.resize(6);
    currentJointPoint_Feedback.positionValue.resize(jointsNum);
    currentJointPoint_Feedback.extPositionValue.resize( 0 );
//    currentJointPoint_FeedbackOld=currentJointPoint_Feedback;
    currentCoordinatePoint_Feedback.configInt.resize(6);
    currentCoordinatePoint_Feedback.positionValue.resize(jointsNum);
    currentCoordinatePoint_Feedback.extPositionValue.resize( 0 );
//    currentCoordinatePoint_FeedbackOld=currentCoordinatePoint_Feedback;
    setVelocityStyle((E_VELOCITY_TYPE)roboParameterIn->jointMoveVelocityType);

    pingPongBuffer=new PingPongBuffer();
    pingPongBuffer->currentReadBufferIndex=0;
    pingPongBuffer->currentReadBufferSubIndex=0;
    pingPongBuffer->currentWriteBufferIndex=0;
    pingPongBuffer->currentWriteBufferSubIndex=0;
    pingPongBuffer->bufferValidateDataLength=2.3/deltaTime;//受到ethercat周期影响
    timeStamp=0;

    resetTrajTime();
    isTrajectoryContiousRun=false;

    createThread();

    #if 1
    int pttime=deltaTime*1000000*15;//unit is ms
    timer_id2[E_TIME2_C1]=TimeTool::getInstance()->creatTimer( "setMotorFeedback", deltaTime*1000000000, pttime,min_v2, max_v2 , false );
    timer_id2[E_TIME2_C2]=TimeTool::getInstance()->creatTimer( "ethercatGetPoint", deltaTime*1000000000, pttime,min_v2, max_v2 , false );
    timer_id2[E_TIME2_C3]=TimeTool::getInstance()->creatTimer( "record wave date", deltaTime*1000000000, pttime,min_v2, max_v2 , false );
    //    timer_id2[E_TIME2_C4]=TimeTool::getInstance()->creatTimer( "C44444----getvelocityik", deltaTime*1000000000, pttime,min_v2, max_v2 , false );
    //    timer_id2[E_TIME2_C5]=TimeTool::getInstance()->creatTimer( "C555", deltaTime*1000000000, pttime,min_v2, max_v2 , false );
    //    timer_id2[E_TIME2_C6]=TimeTool::getInstance()->creatTimer( "C666", deltaTime*1000000000, pttime,min_v2, max_v2 , false );
    //    timer_id2[E_TIME2_C7]=TimeTool::getInstance()->creatTimer( "C777", deltaTime*1000000000, pttime,min_v2, max_v2 , false );
    //    timer_id2[E_TIME2_C8]=TimeTool::getInstance()->creatTimer( "C888", deltaTime*1000000000, pttime,min_v2, max_v2 , false );
    //    timer_id2[E_TIME2_C9]=TimeTool::getInstance()->creatTimer( "C999", deltaTime*1000000000, pttime,min_v2, max_v2 , false );
    //    timer_id2[E_TIME2_C10]=TimeTool::getInstance()->creatTimer( "C10", deltaTime*1000000000, pttime,min_v2, max_v2 , false );
    //    timer_id2[E_TIME2_C11]=TimeTool::getInstance()->creatTimer( "C11", deltaTime*1000000000, pttime,min_v2, max_v2 , false );
      #endif

    #if D_debugFlag
    timer_id3[E_TIME3_C1]=TimeTool::getInstance()->creatTimer( "moveOneStep1-timeAxisMoveOneStep ", deltaTime*1000000000, pttime,min_v3, max_v3 , false );
    timer_id3[E_TIME3_C2]=TimeTool::getInstance()->creatTimer( "moveOneStep2-axisGroupMoveOneStep ", deltaTime*1000000000, pttime,min_v3, max_v3 , false );
    timer_id3[E_TIME3_C3]=TimeTool::getInstance()->creatTimer( "moveOneStep3 -updateTrajTime", deltaTime*1000000000, pttime,min_v3, max_v3 , false );
    timer_id3[E_TIME3_C4]=TimeTool::getInstance()->creatTimer( "moveOneStep4-getJointCommand ", deltaTime*1000000000, pttime,min_v3, max_v3 , false );
    timer_id3[E_TIME3_C5]=TimeTool::getInstance()->creatTimer( "pthread_mutex_lock(&trajMutex)-setfeedback ", deltaTime*1000000000, pttime,min_v3, max_v3 , false );
    timer_id3[E_TIME3_C10]=TimeTool::getInstance()->creatTimer( "axisGroup->setMotorFeedbackDataVector", deltaTime*1000000000, pttime,min_v3, max_v3 , false );
    timer_id3[E_TIME3_C6]=TimeTool::getInstance()->creatTimer( "processJointInstruction ", deltaTime*1000000000, pttime,min_v3, max_v3 , false );
    timer_id3[E_TIME3_C7]=TimeTool::getInstance()->creatTimer( "processLineInstruction ", deltaTime*1000000000, pttime,min_v3, max_v3 , false );
    timer_id3[E_TIME3_C8]=TimeTool::getInstance()->creatTimer( "processJointContinousInstruction", deltaTime*1000000000, pttime,min_v3, max_v3 , false );
    timer_id3[E_TIME3_C9]=TimeTool::getInstance()->creatTimer( "processCartesianContinousInstruction", deltaTime*1000000000, pttime,min_v3, max_v3 , false );

    #endif

    testRoutine();


}

TrajectoryServer::~TrajectoryServer()
{

}

int TrajectoryServer::getAxisSimpleTpInfo(int axisId, SimpleTpInfo &infoOut)
{
    pthread_mutex_lock( &trajMutex);
    double returnFlag=axisGroup->getAxisSimpleTpInfo(axisId,infoOut);
    pthread_mutex_unlock( &trajMutex);
    return returnFlag;
}

int TrajectoryServer::controlAxisSimpleTp(int axisId, int isEnable)
{
    pthread_mutex_lock( &trajMutex);
    double returnFlag=axisGroup->controlAxisSimpleTp(axisId,isEnable);
    pthread_mutex_unlock( &trajMutex);
    return returnFlag;
}

int TrajectoryServer::setAxisSimpleTpTarget(int axisId, double targetPosIn, double maxVelIn, double maxAccIn)
{
    pthread_mutex_lock( &trajMutex);
    double returnFlag=axisGroup->setAxisSimpleTpTarget(axisId,targetPosIn,maxVelIn,maxAccIn);
    pthread_mutex_unlock( &trajMutex);
    return returnFlag;
}

E_AXIS_PLAN_MODE TrajectoryServer::getAxisPlanMode(int axisId)
{
    pthread_mutex_lock( &trajMutex);
    E_AXIS_PLAN_MODE returnFlag=axisGroup->getAxisPlanMode(axisId);
    pthread_mutex_unlock( &trajMutex);
    return returnFlag;
}

int TrajectoryServer::setAxisPlanMode(int axisId, E_AXIS_PLAN_MODE modeIn)
{
    pthread_mutex_lock( &trajMutex);
    E_AXIS_PLAN_MODE tmpOld=getAxisPlanMode(axisId);
    double returnFlag=axisGroup->setAxisPlanMode(axisId,modeIn);
    if(tmpOld!=modeIn && E_AXIS_PLAN_MODE_COMMON==modeIn)
    {
        resetCoordinateTrajPlanCondition();
    }
    pthread_mutex_unlock( &trajMutex);

    return returnFlag;
}

int TrajectoryServer::addCircle(int idIn, std::vector<double> posIn, std::vector<double> centerIn,
                                std::vector<double> normalIn, double vel, int turnsIn, int velTypeIn)
{
    if(false==isInCarvingTp)
    {
        setIsInCarvingMode(true);
    }
    double tmpVel=vel*100;
    if(ENUM_RATIO==velTypeIn)
    {
        tmpVel=vel*roboParameter->lineMaxSpeed;
    }
    return carvingModule->addCircle(idIn,posIn,centerIn,normalIn,tmpVel,roboParameter->lineMaxAcceleration,turnsIn);
}

int TrajectoryServer::resetAlgroCallTimeMeasure()
{
    TimeTool::getInstance()->reset(timer_id2[E_TIME2_C1]);
    TimeTool::getInstance()->reset(timer_id2[E_TIME2_C2]);
    TimeTool::getInstance()->reset(timer_id2[E_TIME2_C3]);
    return 1;
}

int TrajectoryServer::getAlgroCallTimeMeasureDebug(vector<string>& comment, vector<double>& timeInfo)
{
    S_TIME_DATA* dataC1;
    TimeTool::getInstance()->getTimeData(timer_id2[E_TIME2_C1],&dataC1);
    comment.push_back("E_TIME2_C1");
    timeInfo.push_back(dataC1->nowTime);
    timeInfo.push_back(dataC1->maxTime);
    timeInfo.push_back(dataC1->minTime);

    TimeTool::getInstance()->getTimeData(timer_id2[E_TIME2_C2],&dataC1);
    comment.push_back("E_TIME2_C2");
    timeInfo.push_back(dataC1->nowTime);
    timeInfo.push_back(dataC1->maxTime);
    timeInfo.push_back(dataC1->minTime);

    TimeTool::getInstance()->getTimeData(timer_id2[E_TIME2_C3],&dataC1);
    comment.push_back("E_TIME2_C3");
    timeInfo.push_back(dataC1->nowTime);
    timeInfo.push_back(dataC1->maxTime);
    timeInfo.push_back(dataC1->minTime);

    return 1;
}

int TrajectoryServer::setTpNetFeedScale(double scaleIn)
{
    return carvingModule->setNetFeedScale(scaleIn);
}

int TrajectoryServer::getEmcDebugInfo(EmcDebugInfo &debugOut)
{
    return carvingModule->getEmcDebugInfo(debugOut);
}

int TrajectoryServer::emcSetVelLimit(double maxLineVelIn)
{
    return carvingModule->emcSetVelLimit(maxLineVelIn);
}

int TrajectoryServer::isInCarvingMode()
{
    return isInCarvingTp;
}

int TrajectoryServer::setIsInCarvingMode(bool isIn)
{
    qDebug()<<"**********TrajectoryServer::setIsInCarvingMode"<<isIn;
    clearTraj(false);
//    resetAll_exceptStatus();
    isInCarvingTp=isIn;
    return 1;
}

int TrajectoryServer::setCollisionWarnTorque(int axisIndex, double torqueIn)
{
    return dragControl->setCollisionWarnTorque(axisIndex,torqueIn);
}

int TrajectoryServer::setCollisionDetectRatio(double ratioIn)
{
    return dragControl->setCollisionDetectRatio(ratioIn);
}

int TrajectoryServer::getIdealTorque(QVector<double> &torqueOut)
{
    return  axisGroup->getIdealTorque(torqueOut);
}

int TrajectoryServer::setIdealTorque(QVector<double> torqueIn)
{
    return  axisGroup->setIdealTorque(torqueIn);
}

int TrajectoryServer::setGravityVector(vector<double> gravityIn)
{
    pthread_mutex_lock( &trajMutex);
    coordinateManager->setGravityVector(gravityIn);
    pthread_mutex_unlock( &trajMutex);
    return 1;
}

int TrajectoryServer::setToolMass(int toolIndex,double mass, vector<double> center)
{
//    pthread_mutex_lock( &trajMutex);
    coordinateManager->setToolMass(toolIndex,mass,center);
//    pthread_mutex_unlock( &trajMutex);
    return 1;
}

int TrajectoryServer::setSegmentAttribute(int index, JointAttribute attributeIn)
{
    pthread_mutex_lock( &trajMutex);
    coordinateManager->setSegmentAttribute(index,attributeIn);
    pthread_mutex_unlock( &trajMutex);
    return 1;
}

int TrajectoryServer::setDragPara(int axisIndex, DragPara paraIn)
{
    return dragControl->setDragPara(axisIndex,paraIn);
}

int TrajectoryServer::getTrigPosition(QQueue<PointPro> &trigPoseOut)
{
    pthread_mutex_lock( &trajMutex);
    trigPoseOut=motionTrigPose;
    pthread_mutex_unlock( &trajMutex);
    return 1;
}

int TrajectoryServer::stopMotionTrig(int doIndex)
{
    motionTrigStart=false;
    return 1;
}

int TrajectoryServer::setMotionTrigPara(int doIndex, double trigPeriod)
{
    pthread_mutex_lock( &trajMutex);
    motionTrigLoopCount=0;
    motionTrigPose.clear();
    motionTrigDoIndex=doIndex;
    motionTrigPeriod=trigPeriod;
    motionTrigStart=true;
    pthread_mutex_unlock( &trajMutex);
    return 1;
}

int TrajectoryServer::cameraScanTrigLoop()
{
    if(false==motionTrigStart)
    {
        trigTimeCount=0;
        trigStage=0;
        return 0;
    }

    trigTimeCount++;
    switch(trigStage)
    {
    case 0:
    {
        if(D_TRIG_DEBUG)
        {
            qDebug()<<"time trigStage 0,trigTimeCount"<<trigTimeCount;
        }
        ioController->setDo(motionTrigDoIndex,false);
        trigStage=1;
        break;
    }
    case 1:
    {
        if(trigTimeCount>motionTrigPeriod/deltaTime/2)
        {
            trigStage=2;
            if(D_TRIG_DEBUG)
            {
                qDebug()<<"time trigStage 1,trigTimeCount"<<trigTimeCount;
            }
        }
        break;
    }
    case 2:
    {
        if(D_TRIG_DEBUG)
        {
            qDebug()<<"time trigged 2,trigTimeCount"<<trigTimeCount;
        }
        ioController->setDo(motionTrigDoIndex,true);
        trigStage=3;
        break;
    }
    case 3:
    {
        if(D_TRIG_DEBUG)
        {
            qDebug()<<"time trigged 3,record one position,trigTimeCount"<<trigTimeCount;
        }
        //记录当前位置
//        motionTrigPose.enqueue(currentJointPoint_Feedback);
        motionTrigPose.enqueue(currentCoordinatePoint_Feedback);
        trigStage=4;
        break;
    }
    case 4:
    {
        if(trigTimeCount>motionTrigPeriod/deltaTime)
        {
            trigStage=0;

            if(D_TRIG_DEBUG)
            {
                qDebug()<<"time trigged 4,trigTimeCount"<<trigTimeCount;
            }
            trigTimeCount=0;

        }
        break;
    }
    default:
    {
        trigStage=0;
        trigTimeCount=0;
    }
    }

    return 1;
}

int TrajectoryServer::setRobotCoordinateConfig(RobotCoordinateAxisConfig coordinateConfigIn)
{
    pthread_mutex_lock( &trajMutex);
    roboParameter->arm_CeaseTime=coordinateConfigIn.arm_CeaseTime;
    roboParameter->arm_StopTime=coordinateConfigIn.arm_StopTime;
    roboParameter->lineMaxSpeed=coordinateConfigIn.lineMaxSpeed;
    roboParameter->rotate_eqradius=coordinateConfigIn.rotate_eqradius;
    roboParameter->lineMaxAcceleration=coordinateConfigIn.lineMaxAcceleration;
    double totalSpeedRatio=controlSpeedRatio*pidSpeedRatio*safeSpeedRatio*adjustSpeedRatio;
    if(D_STOP_SPEED_RATIO>totalSpeedRatio)
    {
        totalSpeedRatio=0.0;
    }
    carvingModule->emcSetVelLimit(roboParameter->lineMaxSpeed*totalSpeedRatio);
    carvingModule->emcSetRotateLimit(roboParameter->rotate_eqradius*totalSpeedRatio);
    carvingModule->setWaveAcceleration(coordinateConfigIn.lineMaxAcceleration,coordinateConfigIn.lineMaxSpeed,false);
    double returnFlag=axisGroup->setRobotCoordinateConfig(coordinateConfigIn);
    pthread_mutex_unlock( &trajMutex);
    return returnFlag;
}

int TrajectoryServer::ceaseAxisGroup()
{
    pthread_mutex_lock( &trajMutex);
    double returnFlag=axisGroup->ceaseAxisGroup();
    pthread_mutex_unlock( &trajMutex);
    return returnFlag;
}

double TrajectoryServer::getAxisGroupStopTime()
{
    pthread_mutex_lock( &trajMutex);
    double returnFlag=axisGroup->getAxisGroupStopTime();
    pthread_mutex_unlock( &trajMutex);
    return returnFlag;
}

double TrajectoryServer::getAxisGroupVelLimitRatio()
{
    pthread_mutex_lock( &trajMutex);
    double returnFlag=axisGroup->getAxisGroupVelLimitRatio();
    pthread_mutex_unlock( &trajMutex);
    return returnFlag;
}

int TrajectoryServer::initialVehiclePosition( E_VEHICLE_POSITION_TYPE typeIn)
{
    return vehiclePositionManage->initialVehiclePosition(typeIn);
}

int TrajectoryServer::getVehicleMovingCaligrateInfo(VehicleMovingCaligrateInfo &infoOut)
{
    return vehicleJogControl->getVehicleMovingCaligrateInfo(infoOut);
}

int TrajectoryServer::setCalibrateWheelDiameter(double idealMovedLength, double actualMovedLength)
{
    return vehicleJogControl->setCalibrateWheelDiameter(idealMovedLength,actualMovedLength);
}

int TrajectoryServer::setCalibrateWheelDistance(double idealMovedAngle, double actualMovedAngle)
{
    return vehicleJogControl->setCalibrateWheelDistance(idealMovedAngle,actualMovedAngle);
}

int TrajectoryServer::getEncoderValue_belt(int beltIndexIn, int &encoderValueOut)
{
    return beltTrackManager->getEncoderValue_belt(beltIndexIn,encoderValueOut);
}

int TrajectoryServer::getEncoderValue_vision(int visionIndexIn, int &encoderValueOut)
{
    return beltTrackManager->getEncoderValue_vision(visionIndexIn,encoderValueOut);
}

int TrajectoryServer::forceConnectCamera(int visionIndex)
{
    return beltTrackManager->forceConnectCamera(visionIndex);
}

int TrajectoryServer::forceTrigCamera(int visionIndexIn,bool isHighIn)
{

    return beltTrackManager->forceTrigCamera(visionIndexIn,isHighIn);

}

int TrajectoryServer::getImuDeviceData(ImuDataInfo &dataOut)
{
    return vehicleJogControl->getImuDeviceData(dataOut);
}

int TrajectoryServer::setCoupleRelativeDistanceMonitorEnable(int followAxisId, bool isEnableIn)
{
    pthread_mutex_lock( &trajMutex);
    int returnFlag=coupleManagement->setCoupleRelativeDistanceMonitorEnable(followAxisId,isEnableIn);
    pthread_mutex_unlock( &trajMutex);
    return returnFlag;
}

int TrajectoryServer::getCoupleControlInfo(std::vector<CoupleConfigParameter> &infoOut)
{
    pthread_mutex_lock( &trajMutex);
    int returnFlag=coupleManagement->getCoupleControlInfo(infoOut);
    pthread_mutex_unlock( &trajMutex);
    return returnFlag;
}

int TrajectoryServer::setRotateEqradius(double rotate_eqradiusIn)
{
    pthread_mutex_lock( &trajMutex);
    int returnFlag=axisGroup->setRotateEqradius( rotate_eqradiusIn);
    pthread_mutex_unlock( &trajMutex);

    return returnFlag;
}

int TrajectoryServer::setAxisProperty(int axisId, AxisProperty_hmi axisPropertyIn)
{
    pthread_mutex_lock( &trajMutex);
    int returnFlag=axisGroup->setAxisProperty( axisId, axisPropertyIn);
    carvingModule->setJointVel(axisId,axisPropertyIn.maxVelocity,axisPropertyIn.maxAccelation);
    pthread_mutex_unlock( &trajMutex);

    return returnFlag;
}

int TrajectoryServer::setAxisPositionLimit(int axisId, double minPositionIn, double maxPositionIn,
                                           double approachDistanceIn)
{
    pthread_mutex_lock( &trajMutex);
    int returnFlag=axisGroup->setAxisPositionLimit( axisId,  minPositionIn,  maxPositionIn,approachDistanceIn);
    pthread_mutex_unlock( &trajMutex);

    return returnFlag;
}

int TrajectoryServer::getVehicleTrajectoryControlParameter(VehicleTrajectoryControlParameter &infoOut)
{
    return virtualLineControl->getVehicleTrajectoryControlParameter(infoOut);
}

int TrajectoryServer::setVehicleTrajectoryControlParameter(VehicleTrajectoryControlParameter infoIn, int &returnFlag)
{
    return virtualLineControl->setVehicleTrajectoryControlParameter(infoIn,returnFlag);
}

int TrajectoryServer::getAgvBorderInfo(VehicleBorderInfo &infoOut)
{
    return virtualLineControl->getAgvBorderInfo(infoOut);
}

int TrajectoryServer::setAgvBorderRectInfo(BorderRectInfo rectInfoIn, int &returnFlag)
{
    return virtualLineControl->setAgvBorderRectInfo(rectInfoIn,returnFlag);
}

int TrajectoryServer::getVehicleKinematicDimension(VehicleKinematicDimension &infoOut)
{
    return vehicleJogControl->getVehicleKinematicDimension(infoOut);
}

int TrajectoryServer::setVehicleKinematicDimension(VehicleKinematicDimension infoIn, int &returnFlag)
{
     return vehicleJogControl->setVehicleKinematicDimension(infoIn,returnFlag);
}

int TrajectoryServer::getVehicleJogControlParameter(VehicleJogControlParameter &infoOut)
{
    return vehicleJogControl->getVehicleJogControlParameter(infoOut);
}

int TrajectoryServer::setVehicleJogControlParameter(VehicleJogControlParameter infoIn, int &returnFlag)
{
    return vehicleJogControl->setVehicleJogControlParameter(infoIn,returnFlag);
}

int TrajectoryServer::getAgvSheftInfo(int sheftIndex,BorderRectInfo &infoOut)
{
    return virtualLineControl->getAgvSheftInfo(sheftIndex,infoOut);
}

int TrajectoryServer::setAgvSheftRectInfo(int sheftIndex,BorderRectInfo rectInfoIn, int &returnFlag)
{
    return virtualLineControl->setAgvSheftRectInfo(sheftIndex,rectInfoIn,returnFlag);
}

int TrajectoryServer::getVehiclePositionCalibrateInfo(VehiclePosition &infoOut)
{
    return vehiclePositionManage->getVehiclePositionCalibrateInfo(infoOut);
}

int TrajectoryServer::setCameraPointCoordinate(double x, double y, double z)
{
    originCameraPoint_x=x;
    originCameraPoint_y=y;
    calculateCurrentLaserPoint();
    return 1;
}

int TrajectoryServer::getCameraPointInBase(double &x, double &y, double &z)
{
    x=cameraPointInBase_x;
    y=cameraPointInBase_y;
    z=cameraPointInBase_z;
    return 1;
}

int TrajectoryServer::getWeldPositionByCameraTransfer(const PointPro tcpPosition, double x, double y, double z, PointPro &pointOut)
{
    return coordinateManager->getWeldPositionByCameraTransfer(tcpPosition,x,y,z,pointOut);
}

int TrajectoryServer::calculateLaserCameraFrame(int indexIn, std::vector<PointPro> tcpPointList,
                                                std::vector<PointPro> cameraPointList,
                                                LaserCameraCalibrateResult &resultOut)
{
    return coordinateManager->calculateLaserCameraFrame(indexIn,tcpPointList,cameraPointList,resultOut);
}

int TrajectoryServer::getLaserCameraFrameInfo(LaserCameraFrameInfo &infoOut)
{
    return coordinateManager->getLaserCameraFrameInfo(infoOut);
}

int TrajectoryServer::setVelocityStyle(E_VELOCITY_TYPE velocityStypeIn)
{
    int tmpKey;
    pthread_mutex_lock( &trajMutex);
    tmpKey=axisGroup->setVelocityStyle(velocityStypeIn);
    velocityStype_trajServer=velocityStypeIn;
    pthread_mutex_unlock( &trajMutex);
    return tmpKey;
}

int TrajectoryServer::setRobotArmLineMaxAcceleration(double accelerationIn)
{
    int tmpKey;
    pthread_mutex_lock( &trajMutex);
//    carvingModule->setJointAcc(accelerationIn);
    roboParameter->lineMaxAcceleration=accelerationIn;
    tmpKey=axisGroup->setRobotArmLineMaxAcceleration(accelerationIn);
    pthread_mutex_unlock( &trajMutex);
    return tmpKey;
}

int TrajectoryServer::setRobotArmLineMaxVelocity(double velIn)
{
    int tmpKey;
    pthread_mutex_lock( &trajMutex);

    roboParameter->lineMaxSpeed=velIn;
    tmpKey=axisGroup->setRobotArmLineMaxVelocity(velIn);
    pthread_mutex_unlock( &trajMutex);
    return tmpKey;
}

int TrajectoryServer::setRobotArmJointMaxAcceleration(int axisIdIn, double accelerationIn)
{
    int tmpKey;
    pthread_mutex_lock( &trajMutex);
    tmpKey=axisGroup->setRobotArmJointMaxAcceleration(axisIdIn,accelerationIn);
    pthread_mutex_unlock( &trajMutex);
    return tmpKey;
}

int TrajectoryServer::setRobotArmJointMaxVelocity(int axisIdIn, double velIn)
{
    int tmpKey;
    pthread_mutex_lock( &trajMutex);
    tmpKey=axisGroup->setRobotArmJointMaxVelocity(axisIdIn,velIn);
    pthread_mutex_unlock( &trajMutex);
    return tmpKey;
}

int TrajectoryServer::activateCameraCoordinate(int index)
{
    return coordinateManager->activateCameraCoordinate(index);
}

int TrajectoryServer::getCurrentCameraIndex()
{
    return coordinateManager->getCurrentCameraIndex();
}

int TrajectoryServer::pointCoordinateToCartesian_cameraPoint(const PointPro jointPointIn,
                                       const PointPro cameraPointIn, PointPro &worldPointOut)
{
    return coordinateManager->pointCoordinateToCartesian_cameraPoint(jointPointIn,cameraPointIn,worldPointOut);
}

int TrajectoryServer::calculateLaserCameraFrame(int indexIn, std::vector<PointPro> tcpPointList,
         std::vector<PointPro> cameraPointList, std::vector<float> &cameraTransferOut, double &maxErrorOut)
{
    //not used;
//    return coordinateManager->calculateLaserCameraFrame(indexIn,tcpPointList,cameraPointList,cameraTransferOut,maxErrorOut);
}

int TrajectoryServer::getVisionTrackConfigList(std::vector<VisionTrackConfig> &configListOut)
{
    if(ENUM_TRAJECTORY_RUN==basicStatus)
    {
        addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 5067, robotId);
        return -1;
    }
    return beltTrackManager->getVisionTrackConfigList(configListOut);
}

int TrajectoryServer::getBeltTrackConfigList(std::vector<BeltTrackConfig> &configListOut)
{
    if(ENUM_TRAJECTORY_RUN==basicStatus)
    {
        addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 5067, robotId);
        return -1;
    }
    return beltTrackManager->getBeltTrackConfigList(configListOut);
}


int TrajectoryServer::calculateZeroOffset(std::vector<PointPro> pointList, std::vector<double> &zeroOffsetOut, double &maxErrorOut)
{
    return zeroCalculate->calculateZeroOffset(pointList,zeroOffsetOut,maxErrorOut);
}

int TrajectoryServer::controlTypeSwitch(int controlTypeIn)
{
    controlType=controlTypeIn;
    virtualLineControl->controlTypeSwitch(controlTypeIn);
    return 1;
}

int TrajectoryServer::setWaveAcceleration(double accelerationIn,double maxVelIn,bool isRatio)
{
    return carvingModule->setWaveAcceleration(accelerationIn,maxVelIn,isRatio);
}

E_WAVE_STATUS TrajectoryServer::getWaveMotionStatus()
{
    return carvingModule->getWaveMotionStatus();
}

int TrajectoryServer::startWave()
{
    return carvingModule->startWave();
}

int TrajectoryServer::stopWave()
{
    return carvingModule->stopWave();
}

int TrajectoryServer::setWaveType(E_WAVE_TYPE typeIn, double waveWidthIn, double wavePeriodIn)
{
    return carvingModule->setWaveType(typeIn,waveWidthIn,wavePeriodIn);
}

int TrajectoryServer::setWaveMotionInfo(WeaveMotionCondition infoIn)
{
    return carvingModule->setWaveMotionInfo(infoIn);
}

int TrajectoryServer::setVisionTrigObjectLength(int visionIndex, double lengthIn)
{
     return beltTrackManager->setVisionTrigObjectLength(visionIndex,lengthIn);
}

int TrajectoryServer::setTrackOffset(int beltIndex, double offset_x, double offset_y)
{
    return beltTrackManager->setTrackOffset(beltIndex,offset_x,offset_y);
}

int TrajectoryServer::getMotorEncoderPulsePerRound(QVector<double> &pulseOut)
{
    pthread_mutex_lock( &trajMutex);
    int key=axisGroup->getMotorEncoderPulsePerRound(pulseOut);
    pthread_mutex_unlock( &trajMutex);
    return key;
}

int TrajectoryServer::setEncoderReceiverOutput(unsigned short doIn)
{
    return robCtl->setEncoderReceiverOutput(doIn);
}

int TrajectoryServer::setSteeringControlKp(double kpIn, double maxSteerAdjustIn)
{
    return vehicleJogControl->setSteeringControlKp(kpIn,maxSteerAdjustIn);
}

int TrajectoryServer::setVehicleJogDebugFlag(int flagIn)
{
    return vehicleJogControl->setDebugFlag(flagIn);
}

int TrajectoryServer::getCurrentPathInformation(VehiclePosition &startPositionOut, QVector<VehiclePosition> &targetPositionOut,
      int &pointIndexOut, E_NAV_COMMAND &currentPathTypeOut, double &currentPathParameter1Out, double &currentPathParameter2Out)
{
     return virtualLineControl->getCurrentPathInformation(startPositionOut,targetPositionOut,pointIndexOut,currentPathTypeOut,
                                                          currentPathParameter1Out,currentPathParameter2Out);
}

int TrajectoryServer::resetCoupleOffset()
{
    pthread_mutex_lock( &trajMutex);//对轴有操作，需要加锁
    int key=coupleManagement->resetCoupleOffset();
    pthread_mutex_unlock( &trajMutex);
    return key;
}

int TrajectoryServer::getCoupleConfigInfo(int followAxisId, CoupleConfigParameter &coupleParaOut)
{
    pthread_mutex_lock( &trajMutex);
    int key=coupleManagement->getCoupleConfigInfo(followAxisId,coupleParaOut);
    pthread_mutex_unlock( &trajMutex);
    return key;
}

int TrajectoryServer::setCoupleControlEnable(int coupleId, bool isEnableIn)
{
    pthread_mutex_lock( &trajMutex);//对轴有操作，需要加锁
    isCoordinatePlanInitialed=false;
    int key=coupleManagement->setCoupleControlEnable(coupleId,isEnableIn);
    pthread_mutex_unlock( &trajMutex);
    return key;
}

int TrajectoryServer::setCoupleControlOffset(int coupleId, double offfsetValueIn)
{
    pthread_mutex_lock( &trajMutex);
    int key=coupleManagement->setCoupleControlOffset(coupleId,offfsetValueIn);
    pthread_mutex_unlock( &trajMutex);
    return key;
}

int TrajectoryServer::getVisionTrackDebugInfo(int visionIndex, VisionTrackDebugInfo &visionInfoOut)
{
      return beltTrackManager->getVisionTrackDebugInfo(visionIndex,visionInfoOut);
}

int TrajectoryServer::getBeltTrackDebugInfo(int beltIndex, BeltTrackDebugInfo &beltTrackInfoOut)
{
      return beltTrackManager->getBeltTrackDebugInfo(beltIndex,beltTrackInfoOut);
}

int TrajectoryServer::modifyVisionTrackConfig(int visionIndex, VisionTrackConfig configIn)
{
    if(ENUM_TRAJECTORY_RUN==basicStatus)
    {
        addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 5068, robotId);
        return -1;
    }
    return beltTrackManager->modifyVisionTrackConfig(visionIndex,configIn);
}

int TrajectoryServer::modifyBeltTrackConfig(int beltIndex, BeltTrackConfig configIn)
{
    if(ENUM_TRAJECTORY_RUN==basicStatus)
    {
        addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 5068, robotId);
        return -1;
    }
    return beltTrackManager->modifyBeltTrackConfig(beltIndex,configIn);
}

int TrajectoryServer::runVisionTrack(int visionIndex)
{
    return beltTrackManager->runVision(visionIndex);
}

int TrajectoryServer::trigVisionOnce(int visionIndex)
{
    return beltTrackManager->trigVisionOnce(visionIndex);
}

int TrajectoryServer::getVisionData(int visionIndex)
{
    return beltTrackManager->getVisionData(visionIndex);
}

int TrajectoryServer::clearVisionData(int visionIndex)
{
    return beltTrackManager->clearVisionData(visionIndex);
}

int TrajectoryServer::getTrackData(int beltIndex)
{
    int tmpKey=beltTrackManager->getTrackData(beltIndex);
    if(1!=tmpKey)
    {
        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5071, robotId);
    }
    return tmpKey;
}

int TrajectoryServer::beltTrackStart(int beltIndex)
{

    pthread_mutex_lock(&trajMutex);//test
    int tmpKey= beltTrackManager->trackStart(beltIndex);
    if(1==tmpKey)
    {
        double tmpX,tmpY;
        beltTrackManager->getTrackTransionDistance_start(beltIndex,tmpX,tmpY);

        Frame positionFrameCommandNew;
        calculateCurrentCommandPoint_byJointCommand(positionFrameCommandNew);//根据关节算笛卡尔坐标
        positionFrameCommandNew.p[0]-=tmpX/1000.0;
        positionFrameCommandNew.p[1]-=tmpY/1000.0;
        axisGroup->setFrameCommand_one(positionFrameCommandNew);
        isCoordinatePlanInitialed=true;

        qDebug()<<"setFrameCommand_one"<<"positionFrameCommandNew"<<positionFrameCommandNew.p[0]
               <<positionFrameCommandNew.p[1]<<"tmpX"<<tmpX<<"tmpY"<<tmpY;
    }
    pthread_mutex_unlock(&trajMutex);//test
    return tmpKey;
}

int TrajectoryServer::beltTrackEnd(int beltIndex)
{
    pthread_mutex_lock(&trajMutex);//test
    Frame positionFrameCommand_start;
//    positionFrameCommand_start=axisGroup->getPositionFrameCommand();
    calculateCurrentCommandPoint_byJointCommand(positionFrameCommand_start);//根据关节算笛卡尔坐标
    beltTrackManager->setCurrentFrameCommand(beltIndex,positionFrameCommand_start);
    int tmpKey= beltTrackManager->trackEnd(beltIndex);
    isCoordinatePlanInitialed=false;
    pthread_mutex_unlock(&trajMutex);//test
    calculateCurrentPoint();
    return tmpKey;
}

int TrajectoryServer::getEncoderReceiverData(std::vector<EncoderReceiverData> &dataListReturn)
{
    EncoderReceiverData tmpData;
    robCtl->getEncoderReceiverData_iomodule(dataListReturn);

    robCtl->getEncoderReceiverData(tmpData);
    dataListReturn.push_back(tmpData);
    return 1;
}

E_BARRIER_RUN_MODE TrajectoryServer::getBarrierRunMode()
{
    return virtualLineControl->getBarrierRunMode();
}

int TrajectoryServer::setBarrierRunMode(E_BARRIER_RUN_MODE modeIn)
{
    return virtualLineControl->setBarrierRunMode(modeIn);
}

int TrajectoryServer::getVehiclePositionType(E_VEHICLE_POSITION_TYPE &typeOut)
{
    return vehiclePositionManage->getVehiclePositionType(typeOut);
}

int TrajectoryServer::setQrcodeDetectParameter(QrcodeDetectParameter parameterIn)
{
#ifdef D_USE_ROS
    return qrcodeSensorAdapter->setDetectParameter(parameterIn);
#endif
}

int TrajectoryServer::setQrcodeCameraId(int cameraIdIn)
{
    #ifdef D_USE_ROS
    return qrcodeSensorAdapter->setCameraId(cameraIdIn);
    #endif
}

int TrajectoryServer::showQrcodeDebugWindow(int id)
{
    #ifdef D_USE_ROS
    return qrcodeSensorAdapter->showDebugWindow(id);
    #endif
}

int TrajectoryServer::getQrcodeSensorDebugInfo(QrcodeDebugInfo &debugInfoOut)
{
    #ifdef D_USE_ROS
    return qrcodeSensorAdapter->getQrcodeSensorDebugInfo(debugInfoOut);
    #endif
}

int TrajectoryServer::getCu1SensorRawData(int &requestCountOut, double &l1Out, double &l2Out,
                                          double &imageOffsetOut, bool &isSucessOut, int &errorCodeOut)
{
    return cu1Sensor->getRawData(requestCountOut,l1Out,l2Out,imageOffsetOut,isSucessOut,errorCodeOut);
}

int TrajectoryServer::setDebugFlag(int flagIn)
{
    debugFlag=flagIn;
    beltTrackManager->setDebugFlag(flagIn);
    return virtualLineControl->setDebugFlag(flagIn);
}

int TrajectoryServer::setAgvPathFollowStrategy(E_PATH_CONTROL_STRATEGY stragetyIn)
{
    if(E_PATH_CONTROL_STRATEGY_MCCRUM==stragetyIn)
    {
        if(E_VEHICLE_TYPE_TWO_DIFFERENTIAL_DRIVE==vehicleType ||
                E_VEHICLE_TYPE_ONE_STERRING_WHEEL==vehicleType)
        {
            addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5046, robotId);
            return -1;
        }
    }
    return virtualLineControl->setAgvPathFollowStrategy(stragetyIn);
}

int TrajectoryServer::getBarriorDetectMode(E_BARRIER_TETECT_MODE &modeOut)
{
    return virtualLineControl->getBarriorDetectMode(modeOut);
}

int TrajectoryServer::getSafeCollisionInfo(int sensorType, CollisionPara &infoOut)
{
     return virtualLineControl->getSafeCollisionInfo(sensorType,infoOut);
}

int TrajectoryServer::setSafeCollisionInfo(int sensorType, CollisionPara infoIn, int &returnFlag)
{
    return virtualLineControl->setSafeCollisionInfo(sensorType,infoIn,returnFlag);
}

int TrajectoryServer::getMotorControlMode(QVector<E_MOTOR_CONTROL_MODE> &controlModeOut)
{
    pthread_mutex_lock( &trajMutex);//写运动指令时加锁
    int key=axisGroup->getMotorControlMode(controlModeOut);
    pthread_mutex_unlock( &trajMutex);//写完后解锁
    return key;
}

int TrajectoryServer::getMotorControlMode(int axisId, E_MOTOR_CONTROL_MODE &modeOut)
{
    pthread_mutex_lock( &trajMutex);//写运动指令时加锁
    int key=axisGroup->getMotorControlMode(axisId,modeOut);
    pthread_mutex_unlock( &trajMutex);//写完后解锁
    return key;

}

int TrajectoryServer::setCurrentVehicleBorderId(int idIn)
{
    return virtualLineControl->setCurrentVehicleBorderId(idIn);
}

int TrajectoryServer::getCurrentVehicleBorderId()
{
    return virtualLineControl->getCurrentVehicleBorderId();
}

int TrajectoryServer::isVirtualLineTrajectoryEmpty()
{
    return virtualLineControl->isTrajectoryEmpty();
}

int TrajectoryServer::setVirtualLineEnableKpChange(bool isEnable)
{
    if(true==isEnable)
    {
       magneticTrack->exitVmarkMode();
    }
    else
    {
        magneticTrack->changeToVmarkMode();
    }
    return virtualLineControl->setVirtualLineEnableKpChange(isEnable);
}

int TrajectoryServer::clearAllMsg()
{
    axisGroup->clearAllMsg();
//    if(1==roboParameter->isAgvMotionOn)
//    {
//        setCheckVehiclePosition_amcl(false);
//        setCheckVehiclePosition_vmark(false);
//        setCheckVehiclePosition_qrcode(false);
//        virtualLineControl->stop();//报错后需要清空轨迹，重新添加，否则车偏离原来轨迹，再次运行会有问题。
////        if(ENUM_TRAJECTORY_STOPPED==basicStatus)
////        {
////            virtualLineControl->stop();
////        }
//    }

    return 1;
}

int TrajectoryServer::setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE typeIn, bool isCheck)
{
    return vehiclePositionManage->setCheckVehiclePosition(typeIn,isCheck);
}

int TrajectoryServer::setCheckVehiclePosition_amcl(bool isCheck)
{
    return vehiclePositionManage->setCheckVehiclePosition_amcl(isCheck);
}

int TrajectoryServer::setCheckVehiclePosition_vmark(bool isCheck)
{
    return vehiclePositionManage->setCheckVehiclePosition_vmark(isCheck);
}

int TrajectoryServer::setCheckVehiclePosition_qrcode(bool isCheck)
{
    return vehiclePositionManage->setCheckVehiclePosition_qrcode(isCheck);
}

int TrajectoryServer::getVehicleSafeSpeedRatio(double &ratioOut)
{
    if(ENUM_TRAJECTORY_RUN!=basicStatus)
    {
        ratioOut=1;
        return 1;
    }
    switch(motionModel)
    {
        case ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS:
        {

        }
        break;
        case ENUM_MOTION_MODEL_HOMING:
        {

        }
        break;
        case ENUM_MOTION_MODEL_DRAG:
        {

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK:
        {
            magneticTrack->getSafeSpeedRatio(ratioOut);

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK:
        {
            virtualLineControl->getSafeSpeedRatio(ratioOut);
        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_JOG:
        {
            virtualLineControl->getSafeSpeedRatio(ratioOut);

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL:
        {
            vehicleRosControlLoopRealTime();

        }
        break;

        default:
        {

        }

    }
    return 1;
}

int TrajectoryServer::setAgvTargetAllowOffset(float allowLineOffsetIn, float allowAngleOffsetIn)
{
    return virtualLineControl->setTargetAllowOffset(allowLineOffsetIn,allowAngleOffsetIn);
}

int TrajectoryServer::changeToPositionType(E_VEHICLE_POSITION_TYPE typeIn)
{
    E_VEHICLE_POSITION_TYPE tmpType;
    vehiclePositionManage->getVehiclePositionType(tmpType);
    if(E_VEHICLE_POSITION_TYPE_CU1==typeIn && E_VEHICLE_POSITION_TYPE_CU1!=tmpType)
    {
        if(vehiclePositionManage->isReflectNeedInitialPosition())
        {
            PositionInfo positionReturn;
            vehiclePositionManage->getVehiclePosition_specified(positionReturn,E_VEHICLE_POSITION_TYPE_ROS);
            #ifdef D_USE_ROS
            m_rosApi->setReflectInitialVehiclePosition(positionReturn.position);
            #endif
        }

    }
    int tmpReturn=vehiclePositionManage->changeToPositionType(typeIn);
    updateVirtualLinePositionFeedback();
    return tmpReturn;
}

int TrajectoryServer::getTimeConsumed_pointCloudDetect(double &minConsumedOut, double &currentConsumedOut, double &maxConsumedOut)
{
    return virtualLineControl->getTimeConsumed_pointCloudDetect(minConsumedOut,currentConsumedOut,maxConsumedOut);
}

int TrajectoryServer::setVirtualLinePid(double kpDistanceErrorIn, double kpAngleDistanceIn,
                                        double kpOnlyRotationIn, double kpDistanceError_yIn)
{
    return virtualLineControl->setVirtualLinePid(kpDistanceErrorIn,kpAngleDistanceIn,kpOnlyRotationIn,kpDistanceError_yIn);
}

int TrajectoryServer::getTargetCommandVehiclePosition(VehiclePosition &targetOut,int &pointIndexOut)
{
    return virtualLineControl->getTargetCommandVehiclePosition(targetOut,pointIndexOut);
}

int TrajectoryServer::getCarBorder(std::vector<VehiclePosition> &carBorderOut)
{
      return virtualLineControl->getCarBorder(carBorderOut);
}

int TrajectoryServer::getCarBorderOffset(std::vector<VehiclePosition> &carBorderOffsetOut)
{
      return virtualLineControl->getCarBorderOffset(carBorderOffsetOut);
}

int TrajectoryServer::setVehicleBorderOffset(double front_offsetIn, double back_offsetIn, double left_offsetIn,
                                             double right_offsetIn)
{
    return virtualLineControl->setVehicleBorderOffset(front_offsetIn,back_offsetIn,left_offsetIn,right_offsetIn);
}


int TrajectoryServer::setUltrasonicSensorCheckEnable(bool isOn)
{
    return virtualLineControl->setUltrasonicSensorCheckEnable(isOn);
}

int TrajectoryServer::getUltrasonicSensorSafeStatus(std::vector<double> &safeStatusOut, bool &isCheckEnableOut)
{
    return virtualLineControl->getUltrasonicSensorSafeStatus(safeStatusOut,isCheckEnableOut);
}

int TrajectoryServer::getPointCloudSafeStatus(std::vector<string> &sensorNameOut, std::vector<double> &sensorSafeRatioOut, bool &isCheckEnableOut)
{
    return virtualLineControl->getPointCloudSafeStatus(sensorNameOut,sensorSafeRatioOut,isCheckEnableOut);
}

int TrajectoryServer::setPointCloudCheckEnable(bool isOn)
{
     return virtualLineControl->setPointCloudCheckEnable(isOn);
}

int TrajectoryServer::calibrateVehicle(double xOffset, double yOffset, double rOffset)
{
     return vehiclePositionManage->calibrateVehicle(xOffset,yOffset,rOffset);
}

int TrajectoryServer::getVehicleSpeedFeedback(VehicleVelocity &velocityOut)
{
    return vehiclePositionManage->getVehicleSpeedFeedback(velocityOut);
}

int TrajectoryServer::resetCustomOdometer_positionManage()
{
    return vehiclePositionManage->resetCustomOdometer();
}

int TrajectoryServer::getOdometer_positionManage(double &historyOdomOut, double &historyTimeOut, double &customOdomOut,
                                  double &customeTimeOut)
{
    return vehiclePositionManage->getOdometer(historyOdomOut,historyTimeOut,customOdomOut,customeTimeOut);
}

int TrajectoryServer::controllerShutDown()
{
    return vehiclePositionManage->saveOdometerToFile();
}

int TrajectoryServer::setVehicleAcceleration(double accelerationIn)
{
    return virtualLineControl->setAcceleration(accelerationIn);
}

int TrajectoryServer::setVehicleAcceleration_rotate(double accelerationIn)
{
    return virtualLineControl->setAcceleration_rotate(accelerationIn);
}

int TrajectoryServer::getVehicleFollowOffset(double &currentLineOffsetOut, double &currentAngleOffsetOut)
{
    return virtualLineControl->getFollowOffset(currentLineOffsetOut,currentAngleOffsetOut);
}

bool TrajectoryServer::isInitialOk()
{
    return isInitialOkStatus;

}

int TrajectoryServer::getTrackPidOutput(double &anglePidOutput, double &xPidOutput, double &yPidOutput)
{
    return magneticTrack->getPidOutput(anglePidOutput,xPidOutput,yPidOutput);
}

int TrajectoryServer::addVirtualLineTrajectoryCommand(QVector<NavParameter> commandVectorIn)
{
    return virtualLineControl->addTrajectoryCommand(commandVectorIn);
}

int TrajectoryServer::setMaganeticInputIndex(int indexIn)
{
    maganeticInputIndex=indexIn;
    return 1;

}

int TrajectoryServer::writeTagData(int aliasIn, long long cardValueIn)
{
     return robCtl->writeTagData(aliasIn,cardValueIn);
}

int TrajectoryServer::writeTagData(int aliasIn, QVector<unsigned char> cardValueIn)
{
    return robCtl->writeTagData(aliasIn,cardValueIn);
}

int TrajectoryServer::getCurrentTagData(int aliasIn, long long &cardValueOut, bool &isCardDetected)
{
    return robCtl->getCurrentTagData(aliasIn,cardValueOut,isCardDetected);
}

int TrajectoryServer::getCurrentTagData(int aliasIn, QVector<unsigned char> &cardValueOut,bool &isCardDetected)
{
    return robCtl->getCurrentTagData(aliasIn,cardValueOut,isCardDetected);
}

int TrajectoryServer::getMaganeticSensorRawValue(int aliasIn,QVector<bool> &sensorRawValueOut)
{
    return robCtl->getMaganeticSensorRawValue(aliasIn,sensorRawValueOut);
}

int TrajectoryServer::getVehiclePosition_specified(PositionInfo &positionReturn, E_VEHICLE_POSITION_TYPE typeIn)
{
    return  vehiclePositionManage->getVehiclePosition_specified(positionReturn,typeIn);
}

int TrajectoryServer::getVehiclePosition(VehiclePosition &positionReturn)
{
//    positionReturn=vehiclePositionFeedback;
    E_VEHICLE_POSITION_TYPE typeOut;
    vehiclePositionManage->getVehiclePosition_default(positionReturn,typeOut);
    return 1;
}

int TrajectoryServer::getVehiclePosition(VehiclePosition &positionReturn, E_VEHICLE_POSITION_TYPE typeIn)
{

}

int TrajectoryServer::getVehicleVelCommand(bool isJogCommand,double &xMove, double &yMove, double &rotate)
{
    return vehicleJogControl->getVehicleVelCommand(isJogCommand,xMove,yMove,rotate);
}

bool TrajectoryServer::isAbsoluteMotor(int axisId)
{
    return axisGroup->isAbsoluteMotor(axisId);
}

int TrajectoryServer::startGoZero_forUser(int axisId)
{
    qDebug()<<"TrajectoryServer::startGoZero_forUser single in";
    addMsg(ENUM_MSG_REMIND, "MotionServer", "TrajectoryServer", 5091, robotId,axisId);
    goZeroEnableFlag=0;
    homeFinishedCommand.axisId=axisId;
    homeFinishedCommand.isCalibrated=false;
    homeFinishedCommand.isFinished=false;
    homeFinishedCommand.isOneAxis=true;
    homeFinishedCommand.isExit=false;

    servoOff_all();
    sleep(1);
    E_MOTOR_CONTROL_MODE tmpMode;
    axisGroup->getMotorControlMode(axisId,tmpMode);
    axisGroup->startGoZero(axisId);
    if(E_ZERO_TASK_MODE_ETHERCAT==axisGroup->getZeroTaskMode())
    {

        if(E_MOTOR_CONTROL_HOMING==tmpMode)
        {

        }
        else
        {
    //        changeToHomingMode(axisId);
            if(1!=changeToHomingMode(axisId))
            {
                qDebug()<<"failed,changeToHomingMode"<<axisId;
                return -1;
            }

        }
    }
    else
    {
        setMotionServerModel(ENUM_MOTION_MODEL_HOMING);
    }

    servoOn_all();
    sleep(1);
    goZeroEnableFlag=1;
    startGoZero(axisId);//轴变化了,确保下个周期才判断状态。
    homeFinishedCommand.waitCount=0;
    return 1;
}

int TrajectoryServer::startGoZero_forUser()
{
    qDebug()<<"TrajectoryServer::startGoZero_forUser all in";
    addMsg(ENUM_MSG_REMIND, "MotionServer", "TrajectoryServer", 5092, robotId);
    goZeroEnableFlag=0;
    homeFinishedCommand.axisId=-1;
    homeFinishedCommand.isCalibrated=false;
    homeFinishedCommand.isFinished=false;
    homeFinishedCommand.isOneAxis=false;
    homeFinishedCommand.isExit=false;

    servoOff_all();
    sleep(1);
    E_MOTOR_CONTROL_MODE tmpMode;
    axisGroup->getMotorControlMode(0,tmpMode);
    axisGroup->startGoZero();
    if(E_ZERO_TASK_MODE_ETHERCAT==axisGroup->getZeroTaskMode())
    {

        if(E_MOTOR_CONTROL_HOMING==tmpMode)
        {

        }
        else
        {
            changeToHomingMode();
    //        if(1!=changeToHomingMode())
    //        {
    //            return -1;
    //        }
        }
    }
    else
    {
        setMotionServerModel(ENUM_MOTION_MODEL_HOMING);
    }

    servoOn_all();
    sleep(1);
    goZeroEnableFlag=1;
    qDebug()<<"TrajectoryServer::startGoZero_forUser goZeroEnableFlag"<<goZeroEnableFlag;
    return 1;
}

int TrajectoryServer::haltGoZero_forUser()
{
    if(E_ZERO_TASK_MODE_ETHERCAT==axisGroup->getZeroTaskMode())
    {
        haltGoZero();
        axisGroup->haltGoZero();
    }
    else
    {
        axisGroup->haltGoZero();
    }
    return 1;
}

int TrajectoryServer::changeToHomingMode(int axisId)
{
    axisGroup->setMotorControlMode(axisId,E_MOTOR_CONTROL_HOMING);

    QVector<ZeroReturnParameter> tmpZeroReturnInfo;
    axisGroup->getZeroReturnParameter( tmpZeroReturnInfo);
    if(1!=tmpZeroReturnInfo[axisId].isGoZero)
    {
        if(debugFlag)
        {
            qDebug()<< "error, changeToHomingMode ,method not support";
        }

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5042, robotId,axisId);
        return -1;
    }
    robCtl->changeToHomingMode(robotId,axisId,tmpZeroReturnInfo[axisId]);

    setMotionServerModel(ENUM_MOTION_MODEL_HOMING);
    return 1;
}

int TrajectoryServer::changeToHomingMode()
{
    QVector<ZeroReturnParameter> tmpZeroReturnInfo;
    axisGroup->getZeroReturnParameter( tmpZeroReturnInfo);
//    if(tmpZeroReturnInfo[0].goZeroMethod<=0)
//    {
//        if(debugFlag)
//        {
//            qDebug()<< "error, changeToHomingMode ,method not support";
//        }

//        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5041, robotId);
//        return -1;
//    }
    robCtl->changeToHomingMode(robotId,tmpZeroReturnInfo);

    axisGroup->setMotorControlMode(E_MOTOR_CONTROL_HOMING);

    setMotionServerModel(ENUM_MOTION_MODEL_HOMING);
    return 1;
}

int TrajectoryServer::startGoZero(int axisId)
{
    return robCtl->startGoZero(robotId,axisId);

}

int TrajectoryServer::haltGoZero()
{
    return robCtl->haltGoZero(robotId);

}

int TrajectoryServer::getZeroMovingStatus(int axisId, EM_ZERO_MOVING_STATUS &statusOut)
{
    return robCtl->getZeroMovingStatus(robotId,axisId,statusOut);

}

int TrajectoryServer::restorNormalOperationMode(int axisId)
{
    axisGroup->restoreControlMode(axisId);
    #ifndef D_USE_ROS
    robCtl->setMotorTorqueLimit(robotId,axisId,
                               roboParameter->jointAttributeVector[axisId].motorTorqueLimitPositive,
                               roboParameter->jointAttributeVector[axisId].motorTorqueLimitNegative);
    #endif
    return robCtl->restorNormalOperationMode(robotId,axisId);
}

int TrajectoryServer::restorNormalOperationMode()
{
    axisGroup->restoreControlMode();
#ifndef D_USE_ROS
    for(int i=0;i<roboParameter->jointNum;i++)
    {
        robCtl->setMotorTorqueLimit(robotId,i,
                               roboParameter->jointAttributeVector[i].motorTorqueLimitPositive,
                               roboParameter->jointAttributeVector[i].motorTorqueLimitNegative);
    }
#endif
    return robCtl->restorNormalOperationMode(robotId);
}

int TrajectoryServer::getDebugInfo(MotionDebugInfo &infoOut)
{
    AxisGroupDebugInfo axisGroupInfo;
    axisGroup->getDebugInfo(axisGroupInfo);
    motionDebugInfo.axisGroupInfo=axisGroupInfo;

    infoOut=motionDebugInfo;

    infoOut.deltaTime=deltaTime;
    infoOut.currentLineRemainingTime=axisGroup->getLineRemainingTime();
    //infoOut.lineQueueSize;
    infoOut.basicStatus=basicStatus;
    //infoOut.commandQueueSize;
    infoOut.isVelprofileEmpty=isTrajectoryEmpty();
    //infoOut.timeAxisScale0
    //infoOut.commonThreadStage
    //infoOut.realThreadStage
    infoOut.threadId=threadId;
    infoOut.motionServerMode=motionModel;
    infoOut.currentLine=armRunningLineId;//getRunningNumber()


    return 1;
}

int TrajectoryServer::getVirtuleLineDebugInfo(VirtualLineDebugInfo &infoOut)
{
    return virtualLineControl->getVirtuleLineDebugInfo(infoOut);
}

int TrajectoryServer::setMoveToPointFlag()
{
    isMoveToPoint=true;
}

int TrajectoryServer::getWheelData(vector<double> &wheelMoveSpeed, vector<double> &wheelPathLength)
{
    return vehicleJogControl->getWheelData(wheelMoveSpeed,wheelPathLength);
}

int TrajectoryServer::getOdometer(VehicleVelocity &vehicleVelFeedbackOut,
                                  VehiclePosition &vehiclePositionFeedbackOut)
{
    return vehicleJogControl->getOdometer(vehicleVelFeedbackOut,vehiclePositionFeedbackOut);
}

int TrajectoryServer::vehicleJog(double xMove, double yMove, double rotate)
{
//    if(ENUM_MOTION_MODEL_VEHICLE_JOG!=motionModel)
//    {
//        motionModel=ENUM_MOTION_MODEL_VEHICLE_JOG;
//    }
//    return vehicleJogControl->setVehicleVelCommand(xMove,yMove,rotate);
    return vehicleJogControl->vehicleJog(xMove,yMove,rotate);
}

int TrajectoryServer::vehicleAuto(double xMove,double yMove, double rotate)
{
    if(ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL!=motionModel)
    {
        return -1;
    }
    return vehicleJogControl->vehicleAuto(xMove,yMove,rotate);
}

int TrajectoryServer::startVehicleJogMove()
{
    return vehicleJogControl->startRun();

}

int TrajectoryServer::stopVehicleJogMove()
{
    return vehicleJogControl->stop();
}

int TrajectoryServer::setVehicleMagneticMovePid(int idOfPid,TrackPidInfo pidInfo)
{
    return magneticTrack->setVehicleMagneticMovePid(idOfPid,pidInfo);
}



//int TrajectoryServer::getMaganeticDi( QVector<bool> &diOut)
//{
////    return robCtl->getMaganeticDi(robotId,diOut);

//}

int TrajectoryServer::resetVehicleMagneticMove()
{
    magneticTrack->resetAll();
    return 1;

}

int TrajectoryServer::startVehicleMagneticMove( )
{

    return magneticTrack->startRun();
}

int TrajectoryServer::setMageneticPassRfid(long long passRfidIn)
{
    return magneticTrack->setMageneticPassRfid(passRfidIn);
}

int TrajectoryServer::getTrackStatus(E_TRACK_STATUS &statusOut)
{
    return magneticTrack->getTrackStatus(statusOut);
}

int TrajectoryServer::setTrackMode(E_TRACK_MODE modeIn)
{
    return magneticTrack->setTrackMode(modeIn);
}

int TrajectoryServer::setTrackStopCondition(E_TRACK_STOP_CONDITION conditionIn)
{
    return magneticTrack->setStopCondition(conditionIn);
}

int TrajectoryServer::setVehicleIdealSpeed_ratio(double speedIn)
{
    return magneticTrack->setVehicleIdealSpeed_ratio(speedIn);
}

int TrajectoryServer::setVehicleIdealSpeed_mPerS(double speedIn)
{
    return magneticTrack->setVehicleIdealSpeed_mPerS(speedIn);
}

int TrajectoryServer::setMaganeticMovingSpeedRatio(double speedRatioIn)
{
    return magneticTrack->setMaganeticMovingSpeedRatio(speedRatioIn);
}


int TrajectoryServer::setTrajectoryContinuousRun(bool runMethod)
{
    isTrajectoryContiousRun=runMethod;
    return 1;
}

int TrajectoryServer::setMotorControlMode(int axisId, E_MOTOR_CONTROL_MODE modeIn)
{
    pthread_mutex_lock( &trajMutex);//写运动指令时加锁
    int key=axisGroup->setMotorControlMode(axisId,modeIn);
    pthread_mutex_unlock( &trajMutex);//写完后解锁
    robCtl->setMotorControlMode(robotId,axisId,modeIn);
    return key;
}

int TrajectoryServer::moveOneStepByVelocity(int axisId, double velocityIn)
{
    if( basicStatus==ENUM_TRAJECTORY_STOPPING)
    {
        return -1;
    }

    pthread_mutex_lock( &trajMutex);//写运动指令时加锁

    axisGroup->moveOneStepByV(velocityIn,axisId);

    pthread_mutex_unlock( &trajMutex);//写完后解锁
    return 1;

}


int TrajectoryServer::moveOneStepByVelocity(QVector<double> velocity)
{
    if( basicStatus==ENUM_TRAJECTORY_STOPPING)
    {
        qDebug()<<"failed, moveOneStepByVelocity basicStatus"<<basicStatus;
        return -1;
    }

    pthread_mutex_lock( &trajMutex);//写运动指令时加锁
    for(int i=0;i<velocity.size();i++)
    {
        axisGroup->moveOneStepByV(velocity[i],i);
    }
    pthread_mutex_unlock( &trajMutex);//写完后解锁
    return 1;
}

int TrajectoryServer::moveOneStepByPV(double positionIn, double velIn ,int axisId)
{
    if( basicStatus==ENUM_TRAJECTORY_STOPPING)
    {
        return -1;
    }

    pthread_mutex_lock( &trajMutex);//写运动指令时加锁

    axisGroup->moveOneStepByPV(positionIn,velIn,axisId);

    pthread_mutex_unlock( &trajMutex);//写完后解锁
    return 1;
}


int TrajectoryServer::setMotionServerModel(MC_MOTION_MODEL modelIn)
{
    if(modelIn!=motionModel)
    {
//        quickStop();
    }
    else
    {
        return 1;
    }

    if(ENUM_MOTION_MODEL_DRAG==motionModel)
    {
        dragControl->dragOff();
    }


    motionModel=modelIn;
    switch (motionModel)
    {
        case ENUM_MOTION_MODEL_DRAG:
        {
            dragControl->dragOn();
        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_JOG:
        {
            setBarrierRunMode(E_BARRIER_RUN_MODE_COMMON);
        }
        break;

        default :
        {

        }
    }
    return 1;
}

int TrajectoryServer::motionServerLoopRealTime()
{

//    beltTrackLoop();//无轨迹时，补偿生效。有运动时，坐标轨迹靠用户坐标系的移动生效。just for test
    if(jointsNum<=0)
    {
        return -1;
    }
    motionDebugInfo.realThreadStage=E_REAL_THREAD_MOVING_LOOP;

    if(1==roboParameter->isAgvMotionOn)
    {
        #ifdef D_USE_ROS
        rosCommunicate();
        vehicleEncoderOdomUpdate();
        #endif
    }
    if(1==roboParameter->isArmMotionOn)
    {
        trajectoryProcessLoopRealTime();
    }
    //耦合模块工作
    PositionInfo positionReturn;
    vehiclePositionManage->getVehiclePosition_specified(positionReturn,E_VEHICLE_POSITION_TYPE_ROS);
    coupleManagement->setAgvAngleInMap(positionReturn.position.rotate*180.0/M_PI);
    pthread_mutex_lock(&trajMutex);//test//必须加锁保护，里面有对轴的操作．
    coupleManagement->loopRealtimeOnce();
    gantryControl->loopRealtimeOnce();
    cameraScanTrigLoop();//扫描相机触发
    pthread_mutex_unlock(&trajMutex);//test

    updateArmMovingFlag(armRunningLineId,armRunningFlag);

    switch(motionModel)
    {
        case ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS:
        {


        }
        break;
        case ENUM_MOTION_MODEL_HOMING:
        {
    //            axisGroup->zeroManagementLoopRealTime();
            zeroManagementLoopRealTime();

        }
        break;
        case ENUM_MOTION_MODEL_DRAG:
        {
            motionDebugInfo.realThreadStage=E_REAL_THREAD_DRAG_LOOP;
            dragControl->dragLoopRealTime();
        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK:
        {
            magneticTrackLoopRealTime();

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK:
        {
            virtualLineTrackLoopRealTime();
        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_JOG:
        {
            #ifdef D_USE_ROS
            vehicleJogLoopRealTime();
            #endif

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL:
        {
            vehicleRosControlLoopRealTime();

        }
        break;

        default:
        {

        }

    }
    return 1;
}

int TrajectoryServer::vehicleJogLoopRealTime()
{
    motionDebugInfo.realThreadStage=E_REAL_THREAD_VEHICLE_JOG_LOOP;
    if(false==vehicleJogControl->isInitialOk())
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5029,robotId);
        return -1;
    }
    else
    {
//        updateVirtualLinePositionFeedback();消耗cpu，重复使用了．
//        double tmpOut;
        if(ENUM_MOTION_MODEL_VEHICLE_JOG==motionModel)
        {
            updateVirtualLinePositionFeedback();
            double tmpOut;
            virtualLineControl->sensorProtectLoop(false,tmpOut);
            vehicleJogControl->setJogSafeRatio(tmpOut);
        }
        vehicleJogUpdate();

    }
    return 1;
}

int TrajectoryServer::vehicleRosControlLoopCommonTime()
{
    //getVehicleVelocityComand
   VehicleVelocity velocityComamnd;
    #ifdef D_USE_ROS
    m_rosApi->getVehicleVelocityComand(userInfo,robotId,velocityComamnd);
//    vehicleJogControl->vehicleAuto(velocityComamnd.x_move,velocityComamnd.y_move,velocityComamnd.z_rotate);
    vehicleJogControl->setRosVelCommand(velocityComamnd.x_move,velocityComamnd.y_move,velocityComamnd.z_rotate);
    #endif

    return 1;

}

int TrajectoryServer::vehicleRosControlLoopRealTime()
{
//    vehicleJogControl->calculateRosVelCommandSmooth();
    return vehicleJogLoopRealTime();

}

int TrajectoryServer::magneticTrackLoopCommonTime()
{
    return 1;
}

int TrajectoryServer::magneticTrackLoopRealTime()
{
    motionDebugInfo.realThreadStage=E_REAL_THREAD_MAGANETIC_TRACK_LOOP;
    if(false==magneticTrack->isInitialOk())
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5027,robotId);
        return -1;
    }
    else
    {
        robCtl->getMaganeticSensorRawValue(magneticTrack->getFrontMaganetic_deviceAlias(),
                                           sensorInfo.magneticData_front);
        robCtl->getMaganeticSensorRawValue(magneticTrack->getBackMaganetic_deviceAlias()
                                           ,sensorInfo.magneticData_back);
        robCtl->getCurrentTagData(magneticTrack->getFrontRfid_deviceAlias()
                                  ,sensorInfo.rfidData_front,sensorInfo.isRfidDetected_front);
        robCtl->getCurrentTagData(magneticTrack->getBackRfid_deviceAlias()
                                  ,sensorInfo.rfidData_back,sensorInfo.isRfidDetected_back);
        robCtl->getRobotDi(robotId,maganeticInputIndex,sensorInfo.input);
        //转角
        getAgvSteeringPositionFeedback(sensorInfo.steeringAngle);

        //激光安全系数获取.设置激光障碍物检测方向
        updateVirtualLinePositionFeedback(magneticTrack->getVehicleIdealSpeed_mPerS());
        double tmpOut;
        virtualLineControl->sensorProtectLoop(true,tmpOut);
        magneticTrack->setLaserSafeRatio(tmpOut);

        magneticTrack->setStart();
        QVector<double> wheelVelToReturn;
        QVector<double> steeringAngleOut;
        double xMove, yMove, rotate;
        E_TRACK_LOOP_RESULT tmpResult=magneticTrack->trackLoop(sensorInfo,wheelVelToReturn,steeringAngleOut,xMove, yMove, rotate);
        if(E_TRACK_LOOP_RESULT_ERROR==tmpResult )
        {
            wheelVelToReturn.clear();
            pthread_mutex_lock( &trajMutex);//写运动指令时加锁
            axisGroup->setAgvAxisVel(wheelVelToReturn);
            pthread_mutex_unlock( &trajMutex);//写完后解锁
            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5028,robotId);
            return -2;
        }
        else if(E_TRACK_LOOP_RESULT_NORMAL==tmpResult || E_TRACK_LOOP_RESULT_ZERO_HALT==tmpResult )
        {
            pthread_mutex_lock( &trajMutex);//写运动指令时加锁
            axisGroup->setAgvAxisVel(wheelVelToReturn);
            axisGroup->setAgvSteeringPosition(steeringAngleOut);
            pthread_mutex_unlock( &trajMutex);//写完后解锁
            return 1;
        }
    }
    return 1;
}


int TrajectoryServer::virtualLineTrackLoopCommonTime()
{
    #ifdef D_USE_ROS
    //检查是否在创建地图中
    if(true==m_rosApi->isInCreatingMapStatus())
    {
        addMsg(ENUM_MSG_REMIND,"MotionServer","trajectory",5044,robotId);
        return -1;
    }
    virtualLineControl->processLoopCommon();
    #endif
    return 1;
}

#define D_REFLECT_CEASE_TIME 3
#define D_REFLECT_RUN_TIME 5
int TrajectoryServer::virtualLineTrackLoopRealTime()
{



    //检查反光条位置；
    E_VEHICLE_POSITION_TYPE tmpType;
    vehiclePositionManage->getVehiclePositionType(tmpType);
    if(E_VEHICLE_POSITION_TYPE_CU1==tmpType)
    {
        static bool tmpIsReflectPositionOk_old=true;
        bool tmpIsReflectPositionOk=true;
        vehiclePositionManage->getReflectPositionStatus(tmpIsReflectPositionOk);
        if(false==tmpIsReflectPositionOk && true==tmpIsReflectPositionOk_old)
        {
            virtualLineControl-> setAdjustSpeedRatio(0.0,D_REFLECT_CEASE_TIME);
            addMsg(ENUM_MSG_WARNING,"MotionServer","trajectory",5061,robotId);
        }
        else if(true==tmpIsReflectPositionOk && false==tmpIsReflectPositionOk_old )//&& ENUM_TRAJECTORY_RUN==basicStatus 导致没有恢复
        {
            virtualLineControl-> setAdjustSpeedRatio(1.0,D_REFLECT_RUN_TIME);
            addMsg(ENUM_MSG_WARNING,"MotionServer","trajectory",5062,robotId);
        }
        tmpIsReflectPositionOk_old=tmpIsReflectPositionOk;
    }
    else if(E_VEHICLE_POSITION_TYPE_VMARK==tmpType)
    {
        static bool tmpIsVmarkPositionOk_old=true;
        bool tmpIsVmarkPositionOk=true;
        vehiclePositionManage->getReflectPositionStatus(tmpIsVmarkPositionOk);
        if(false==tmpIsVmarkPositionOk && true==tmpIsVmarkPositionOk_old)
        {
            virtualLineControl-> setAdjustSpeedRatio(0.0,D_REFLECT_CEASE_TIME);
            addMsg(ENUM_MSG_WARNING,"MotionServer","trajectory",5069,robotId);
        }
        else if(true==tmpIsVmarkPositionOk && false==tmpIsVmarkPositionOk_old )//&& ENUM_TRAJECTORY_RUN==basicStatus 导致没有恢复
        {
            virtualLineControl-> setAdjustSpeedRatio(1.0,D_REFLECT_RUN_TIME);
            addMsg(ENUM_MSG_WARNING,"MotionServer","trajectory",5070,robotId);
        }
        tmpIsVmarkPositionOk_old=tmpIsVmarkPositionOk;
    }

    //时间分析
    struct timespec startTime,endTime1,endTime2,endTime3,endTime4,endTime5,endTime6
            ,endTime5a,endTime5b,endTime5c;
    clock_gettime(CLOCK_MONOTONIC, &startTime);
    double currentConsumed;

    updateVirtualLinePositionFeedback();

    clock_gettime(CLOCK_MONOTONIC, &endTime1);
    currentConsumed=1000000000*(endTime1.tv_sec-startTime.tv_sec)+endTime1.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(100==debugFlag)
    {
        //cpu高时，达到５ｍｓ
        qDebug()<<"TrajectoryServer-virtualLineTrackLoopRealTime endTime1:"<<currentConsumed;
    }

    virtualLineControl->processLoopRealTime();

    clock_gettime(CLOCK_MONOTONIC, &endTime3);
    currentConsumed=1000000000*(endTime3.tv_sec-startTime.tv_sec)+endTime3.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(100==debugFlag)
    {
        qDebug()<<"TrajectoryServer-virtualLineTrackLoopRealTime endTime3:"<<currentConsumed;
    }

    magneticTrack->setTrackMode(E_TRACK_MODE_VIRTUAL_LINE);


    if(false==magneticTrack->isInitialOk())
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5027,robotId);
        return -1;
    }
    else
    {
        BiasControlInfo controlInfoOut;
        virtualLineControl->getBias(controlInfoOut);
        sensorInfo.deltaDistanceX_virtualLine=controlInfoOut.deltaDistanceX;
        sensorInfo.deltaDistanceY_virtualLine=controlInfoOut.deltaDistanceY;
        sensorInfo.deltaAngle_virtualLine=controlInfoOut.deltaAngle;
        sensorInfo.absoluteDistance_virtualLine=controlInfoOut.absoluteDistance;
        sensorInfo.absoluteAngle_virtualLine=controlInfoOut.absoluteAngle;
        sensorInfo.forward_x=controlInfoOut.forward_x;
        sensorInfo.forward_y=controlInfoOut.forward_y;
        sensorInfo.forward_r=controlInfoOut.forward_r;
//        qDebug()<<"forward"<<sensorInfo.forward_x<<sensorInfo.forward_y<<sensorInfo.forward_r;

        if(true==controlInfoOut.isPidStop)
        {
            magneticTrack->resetAll();

        }
        else
        {
            magneticTrack->setStart();
        }
        magneticTrack->setVehicleDriveDirection(controlInfoOut.isPositiveDirection);//前进后退不同车型有区别,虚拟跟随没使用这个参数
        QVector<double> wheelVelToReturn;
        QVector<double> steeringAngleOut;
        double xMove, yMove, rotate;
        E_TRACK_LOOP_RESULT tmpResult=magneticTrack->trackLoop(sensorInfo,wheelVelToReturn,steeringAngleOut,
                                                               xMove,yMove,rotate);

        if(E_TRACK_LOOP_RESULT_NORMAL==tmpResult)
        {
            vehicleJogControl->vehicleAuto(xMove,yMove,rotate);
            double tmpFeedX,tmpFeedY,tmpFeedR;
            magneticTrack->getVirtualLineTrack_feedfowardOutput(tmpFeedX,tmpFeedY,tmpFeedR);
            vehicleJogControl->setVirtualLineTrack_feedfowardOutput(tmpFeedX,tmpFeedY,tmpFeedR);
        }
        else
        {
            vehicleJogControl->vehicleAuto(0,0,0);
        }
        vehicleJogControl->startRun();

        clock_gettime(CLOCK_MONOTONIC, &endTime4);
        currentConsumed=1000000000*(endTime4.tv_sec-startTime.tv_sec)+endTime4.tv_nsec-startTime.tv_nsec;
        currentConsumed=currentConsumed/1000.0;
        if(100==debugFlag)
        {
            qDebug()<<"TrajectoryServer-virtualLineTrackLoopRealTime endTime4:"<<currentConsumed;
        }


        vehicleJogLoopRealTime();

        clock_gettime(CLOCK_MONOTONIC, &endTime5);
        currentConsumed=1000000000*(endTime5.tv_sec-startTime.tv_sec)+endTime5.tv_nsec-startTime.tv_nsec;
        currentConsumed=currentConsumed/1000.0;
        if(100==debugFlag)
        {
            //cpu高时，达到５ｍｓ
            qDebug()<<"TrajectoryServer-virtualLineTrackLoopRealTime endTime5:"<<currentConsumed;
        }

        //舵轮转向过渡，转向角覆盖vehicleJogControl生成的转向角度
        if(E_BIAS_TYPE_STEERING_WAIT==virtualLineControl->getCalculateBiasType())
        {
            QVector<double> steeringAngle;
            virtualLineControl->getTrajectoryPoint_steeringAngle(steeringAngle);
            pthread_mutex_lock( &trajMutex);//写运动指令时加锁
            axisGroup->setAgvSteeringPosition(steeringAngle);
            pthread_mutex_unlock( &trajMutex);//写完后解锁
        }


    }
    clock_gettime(CLOCK_MONOTONIC, &endTime6);
    currentConsumed=1000000000*(endTime6.tv_sec-startTime.tv_sec)+endTime6.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(100==debugFlag)
    {
        qDebug()<<"TrajectoryServer-virtualLineTrackLoopRealTime endTime6:"<<currentConsumed;
    }
    return 1;
}

int TrajectoryServer::updateVirtualLinePositionFeedback(double velocity_x_in)
{
    //时间分析
    struct timespec startTime,endTime1,endTime2,endTime3,endTime4,endTime5,endTime6
            ,endTime5a,endTime5b,endTime5c;
    clock_gettime(CLOCK_MONOTONIC, &startTime);
    double currentConsumed;

    #ifdef D_USE_ROS
    //点云更新
    //有时会ｃｏｒｅdump，也会导致a.exec()消息循环coredump. 有一次编译后，可以正常运行，有一次加大堆栈为１００Ｍ可以正常运行。
//    std::vector<PointCloudData_3D> *pointDataOut=new std::vector<PointCloudData_3D>;
//    m_rosApi->getAllPointCloud_translated(*pointDataOut);
    QVector<PointCloudData_3D> *pointDataOut=new QVector<PointCloudData_3D>;
    m_rosApi->getAllPointCloud_translated_qvector(*pointDataOut);

    clock_gettime(CLOCK_MONOTONIC, &endTime1);
    currentConsumed=1000000000*(endTime1.tv_sec-startTime.tv_sec)+endTime1.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(100==debugFlag)
    {
        qDebug()<<"555TrajectoryServer-updateVirtualLinePositionFeedback time1:"<<currentConsumed;
    }

    virtualLineControl->setPointCloudData(*pointDataOut);
    delete pointDataOut;
    #endif

    clock_gettime(CLOCK_MONOTONIC, &endTime2);
    currentConsumed=1000000000*(endTime2.tv_sec-startTime.tv_sec)+endTime2.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(100==debugFlag)
    {
        //5ms消耗时间
        qDebug()<<"55TrajectoryServer-updateVirtualLinePositionFeedback time2:"<<currentConsumed;
    }

    virtualLineControl->setServoStatus(isServoOn());
    VehiclePosition tmpPosition;
    E_VEHICLE_POSITION_TYPE typeOut;
    vehiclePositionManage->getVehiclePosition_default(tmpPosition,typeOut);
    VehicleVelocity tmpVehicleVelFeedback;
    VehiclePosition tmpVehiclePositionFeedback;

    getOdometer(tmpVehicleVelFeedback,tmpVehiclePositionFeedback);
    VehicleVelocity tmpVehicleVelCommand;
    vehicleJogControl->getVehicleVelCommand(false,tmpVehicleVelCommand.x_move,tmpVehicleVelCommand.y_move,
                                            tmpVehicleVelCommand.z_rotate);

    clock_gettime(CLOCK_MONOTONIC, &endTime3);
    currentConsumed=1000000000*(endTime3.tv_sec-startTime.tv_sec)+endTime3.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(100==debugFlag)
    {
        qDebug()<<"5TrajectoryServer-updateVirtualLinePositionFeedback time3:"<<currentConsumed;
    }

    QVector<double> steeringPosition;
    getAgvSteeringPositionFeedback(steeringPosition);
    if(ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK==motionModel)
    {
        VehicleVelocity magVehicleVelCommand;
        magVehicleVelCommand.x_move=velocity_x_in;
        magVehicleVelCommand.y_move=0;
        magVehicleVelCommand.z_rotate=0;
        virtualLineControl->setPositionFeedback(tmpPosition,tmpVehicleVelFeedback,steeringPosition);
        virtualLineControl->setVehicleVelCommand(magVehicleVelCommand);
    }
    else
    {
        virtualLineControl->setPositionFeedback(tmpPosition,tmpVehicleVelFeedback,steeringPosition);
        virtualLineControl->setVehicleVelCommand(tmpVehicleVelCommand);
    }

    clock_gettime(CLOCK_MONOTONIC, &endTime4);
    currentConsumed=1000000000*(endTime4.tv_sec-startTime.tv_sec)+endTime4.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(100==debugFlag)
    {
        qDebug()<<"6TrajectoryServer-updateVirtualLinePositionFeedback time4:"<<currentConsumed;
    }

    return 1;

}

int TrajectoryServer::motionServerLoopCommon()
{
    if(jointsNum<=0)
    {
        return -1;
    }
    motionDebugInfo.commonThreadStage=E_COMMON_THREAD_IN;

    calibrateCheck();

    if(1==roboParameter->isAgvMotionOn)
    {
        #ifdef D_USE_ROS
        virtualLineTrackLoopCommonTime();
        #endif
    }
    if(1==roboParameter->isArmMotionOn)
    {
        trajectoryProcessLoopCommon();
//        velocitySlowdownLoopRealtime();
    }



    switch(motionModel)
    {
        case ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS:
        {


        }
        break;
        case ENUM_MOTION_MODEL_DRAG:
        {
            motionDebugInfo.commonThreadStage=E_COMMON_THREAD_DRAG_LOOP;
            dragControl->dragLoopCommon();
        }
        break;
        case ENUM_MOTION_MODEL_HOMING:
        {
            motionDebugInfo.commonThreadStage=E_COMMON_THREAD_HOMING_LOOP;
//            axisGroup->zeroManagementLoopCommonTime();
            zeroManagementLoopCommonTime();
        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK:
        {

            magneticTrackLoopCommonTime();
        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK:
        {


        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_JOG:
        {
            #ifdef D_USE_ROS
            vehicleJogLoopCommonTime();
            #endif
        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL:
        {

            vehicleRosControlLoopCommonTime();
        }
        break;

        default:
        {

        }
    }
    motionDebugInfo.commonThreadStage=E_COMMON_THREAD_END;
    return 1;
}

int TrajectoryServer::trajectoryProcessLoopRealTime()
{
    motionDebugInfo.realThreadStage=E_REAL_THREAD_TRAJECTORY_LOOP;

    timeAxisMoveOneStep();

    if(isInCarvingTp)
    {

        if(1==isMoveConditionOk_servo())
        {
            moveOneStep_carvingTp();
            moveOneStep_simpleTp();
        }

    }
    else
    {
        if(1==isMoveConditionOk_servo())
        {
            if(1==isMoveConditionOk_axis())
            {
                moveOneStep();
            }
            moveOneStep_simpleTp();
        }

    }




    if(roboParameter->isCalculateIdealJointTorque)
    {
        //计算动力学
        QVector<double> q_in,tmpQdot,tmpQdotdot,f_tool,torqueOut;
        getJointFeedback(q_in, tmpQdot,tmpQdotdot,torqueOut);
        //todo
        for(int i=0;i<q_in.size();i++)
        {
            tmpQdot[i]=0;
            tmpQdotdot[i]=0;
        }
        if(1==coordinateManager->getIdealJointTorque(q_in,tmpQdot,tmpQdotdot,f_tool,torqueOut))
        {
            setIdealTorque(torqueOut);
        }


        //碰撞检测
        if(ENUM_MOTION_MODEL_DRAG!=motionModel)
        {
            int tmpKey=dragControl->collisionDetectLoop();
            if(tmpKey>=0)
            {
                    allCease();
                    addMsg(ENUM_MSG_WARNING,"MotionServer","trajectory",5081,robotId,tmpKey+1);

            }
        }
    }


    return 1;
}
int TrajectoryServer::trajectoryProcessLoopCommon()
{

    motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_LOOP;
    if(basicStatus==ENUM_TRAJECTORY_RUN || basicStatus==ENUM_TRAJECTORY_STOPPING ||
            basicStatus==ENUM_TRAJECTORY_TOSTOP || basicStatus==ENUM_TRAJECTORY_CEASING
            || basicStatus==ENUM_TRAJECTORY_TOCEASE)
     {
        laserLoseRescueLogic();
        //时间分析
        struct timespec startTime,endTime;
        clock_gettime(CLOCK_MONOTONIC, &startTime);


        int tmpKey;
        if(0)
        {
            tmpKey=processQueueForCarvingModule();
        }
        else
        {
            tmpKey=calculateTrajectoryPath_cnt();
        }


        if(0>tmpKey)
        {
//            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5060,robotId);
            qDebug()<<"0>calculateTrajectoryPath()";
            allStop();
        }
        else if(1==tmpKey)
        {
                double currentConsumed;
                clock_gettime(CLOCK_MONOTONIC, &endTime);
                currentConsumed=1000000000*(endTime.tv_sec-startTime.tv_sec)+endTime.tv_nsec-startTime.tv_nsec;
                currentConsumed=currentConsumed/1000.0;
                if(0)
                {
                    qDebug()<<"calculateTrajectoryPath consumetime:"<<currentConsumed<<"us";
                }
        }



     }
    else if(basicStatus==ENUM_TRAJECTORY_STOPPED)
    {

    }

    return 1;
}

int TrajectoryServer::velocitySlowdownLoopRealtime(bool isTpValid)
{
//    if(ENUM_CONTROL_T1!=controlType)
//    {
//        return 0;
//    }

//    D_COMMON_LOOP_PERIOD slowdownTickcount
    if(slowdownTickcount>0)
    {
        slowdownTickcount--;
    }

    double currentVelRatio=getAxisGroupVelLimitRatio();
    if(axisGroup->isInDecartTrajStatus() || isTpValid)
    {
        double warnSpeedRatio=roboParameter->arm_warnSpeedRatio;//roboParameter->t1RatioLimit 0.9
        if(currentVelRatio>warnSpeedRatio)
        {
            double validTime=0.05;
            double tmpNewRatio=adjustSpeedRatio*roboParameter->arm_warnSlowRatio;//0.1
            if(0==slowdownTickcount)
            {
                slowdownTickcount=validTime/deltaTime;
                setAdjustSpeedRatio(tmpNewRatio,validTime);
                addMsg(ENUM_MSG_REMIND,"MotionServer","trajectory",5074,robotId);
            }

            qDebug()<<"----currentVelRatio>warnSpeedRatio,velocitySlowdownLoopCommon currentVelRatio"<<currentVelRatio
                   <<"setAdjustSpeedRatio"<<tmpNewRatio;
        }
//        qDebug()<<"-----isInDecartTrajStatus,velocitySlowdownLoopCommon currentVelRatio"<<currentVelRatio;
    }
    else
    {
        if(adjustSpeedRatio<1)
        {
            addMsg(ENUM_MSG_REMIND,"MotionServer","trajectory",5075,robotId);
            setAdjustSpeedRatio(1,0.1);
        }
    }


    return 1;


}

int TrajectoryServer::velocitySlowdownLoopRealtime_imdeat(bool isTpValid)
{

    double currentVelRatio=getAxisGroupVelLimitRatio();
    if(axisGroup->isInDecartTrajStatus() || isTpValid)
    {
        //太接近奇异点会导致调整倍率不够，从而超过速度。球面边缘，速度突变不是很厉害，可以成功降速。
        if(currentVelRatio>roboParameter->arm_warnSpeedRatio)
        {
            slowdownTickcount=0.05/deltaTime;
            axisGroup->setNewTimeAxisVelLimitRatio_coordAxis(roboParameter->arm_warnSpeedRatio/currentVelRatio);

            addMsg(ENUM_MSG_REMIND,"MotionServer","trajectory",5074,robotId);
        }
        else
        {

            if(1==slowdownTickcount)
            {//需要延迟恢复速度，否则速度会震荡厉害。
                axisGroup->restoreTimeAxisVelLimitRatio_coordAxis(roboParameter->arm_warnSpeedRatio/currentVelRatio);
                addMsg(ENUM_MSG_REMIND,"MotionServer","trajectory",5075,robotId);
            }
            if(slowdownTickcount>0)
            {
                slowdownTickcount--;
            }

        }
//        qDebug()<<"2222after moved currentVelRatio"<<currentVelRatio;

    }
    else
    {
        axisGroup->resetTimeAxisVelLimitRatio_coordAxis();
    }


    return 1;
}

int TrajectoryServer::beltTrackLoop()
{
    if(false==beltTrackManager->isUseThisModule())
    {
        return 0;
    }

    Frame tmpPositionFrame;
    QVector<PointPro> deltaPoint;//跟踪启动和停止时的过渡补偿
    QVector<E_MOVE_STAGE> stageOut;
    beltTrackManager->loopRealtimeOnce(deltaPoint,stageOut);
    for(int i=0;i<stageOut.size();i++)
    {
        if(E_MOVE_STAGE_NONE!=stageOut[i])//启动－工作－停止阶段
        {
            axisGroup->getCoordinateAxisPositionCommand(tmpPositionFrame);//不能根据关节算，而要根据轨迹规划算．
        //    qDebug()<<"delta"<<deltaPoint.positionValue[0]<<deltaPoint.positionValue[1];
            tmpPositionFrame.p[0]+=deltaPoint[i].positionValue[0]/1000.0;
            tmpPositionFrame.p[1]+=deltaPoint[i].positionValue[1]/1000.0;
        //    positionFrameCommandBefore.p[2]+=deltaPoint.positionValue[2]/1000.0;

            PointPro tmpPoint=coordinateManager->frameToPointPro_CART(tmpPositionFrame);
            axisGroup->moveOneStep_byUserCoordinateCommand(tmpPoint);
            if(0)
            {
                qDebug()<<"belt loop"<<"tmpPoint"<<tmpPoint.positionValue[0]<<tmpPoint.positionValue[1]
                         <<tmpPoint.positionValue[2]<<tmpPoint.positionValue[3]<<tmpPoint.positionValue[4]
                         <<tmpPoint.positionValue[5]
                       <<"delta"<<deltaPoint[i].positionValue[0]<<deltaPoint[i].positionValue[1]
                      <<"stage"<<stageOut[i]<<"i"<<i;
            }
        }
        else//跟踪过程中暂停和重新启动控制
        {
            Frame tmpOverLayFrame;
            double tmpDurationLeft;
            if(1==beltTrackManager->moveOneStep_FrameCommand(i,tmpOverLayFrame,tmpDurationLeft))
            {
                axisGroup->moveOneStep_byWorldCoordinate(tmpOverLayFrame,true,tmpDurationLeft);
            }
        }

    }

    return 1;


}

 int TrajectoryServer::setBasicStatus(MC_TRAJECTORY_STATUS statusIn, int comment)
 {
//     qDebug()<<"TrajectoryServer::setBasicStatus"<<statusIn<<"comment"<<comment;
     if(ENUM_TRAJECTORY_STOPPED==basicStatus && ENUM_TRAJECTORY_CEASED==statusIn)
     {
         return 0;
     }
     basicStatus=statusIn;
     return 1;
 }

int TrajectoryServer::setCheckMode(E_CHECK_MODE modeIn)
{
   checkMode= modeIn;
   axisGroup->setCheckMode(modeIn);
    return 1;
}
int TrajectoryServer::servoOn()
{
    qDebug()<<"TrajectoryServer::servoOnCurrentRobot";
    if(ENUM_TRAJECTORY_RUN==basicStatus)
    {
        qDebug()<<"warn: ENUM_TRAJECTORY_RUN==basicStatus can not change motor status";
        return -1;
    }
    if(jointsNum<=0 ||  false==isMotorCommandValid)
    {
        return -1;
    }

    while(false==robCtl->isInitialOk())
    {
        static int tmpTryCount=0;
//        qDebug()<<"servo on waitting";
        usleep(100000);
        tmpTryCount++;
        if(tmpTryCount>100)
        {
            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5066,robotId);
            return -1;
        }
    }

    //安全判断
    if(1==allowServonCheck)
    {
        QVector<double> jointsPosition_c, jointsVel_c,jointAcc_c, jointTorque_c;
        QVector<double> jointsPosition_f, jointsVel_f,jointAcc_f, jointTorque_f;
        getJointCommand(jointsPosition_c, jointsVel_c,jointAcc_c, jointTorque_c);
        getFeedbackJoint(jointsPosition_f, jointsVel_f,jointAcc_f, jointTorque_f);
        for(int i=0;i<jointsPosition_c.size();i++)
        {
            double tmpDiff=fabs(jointsPosition_c[i]-jointsPosition_f[i]);
            if(tmpDiff>allowServonCheckDifference)
            {
                qDebug()<<"error---------------,servon error,反馈与命令值偏差到过允许，axisid="<<i
                       <<"tmpDiff="<<tmpDiff;
                addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5036,robotId,i,tmpDiff);
                return -1;
            }
        }

    }
    if(false==isAxisZeroOk() && false==allowSingleAxisJogFlag)
    {
        addMsg(ENUM_MSG_WARNING,"MotionServer","trajectory",5064,robotId);
        return -1;
    }

    resetAll_exceptStatus();
    pthread_mutex_lock(&trajMutex);//test
    bool tmpIsMustOn;
    for(int i=0;i<axisLogicalTypeVector.size();i++)
    {

        tmpIsMustOn= isMustOnAxis(axisLogicalTypeVector[i]);
        if(true==tmpIsMustOn)
        {
           robCtl->servoOnRobotAxis( robotId,i );
        }

    }

    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}

int TrajectoryServer::servoOn_all()
{
    if(ENUM_TRAJECTORY_RUN==basicStatus)
    {
        qDebug()<<"warn: ENUM_TRAJECTORY_RUN==basicStatus can not change motor status";
        return -1;
    }
    if(jointsNum<=0 || false==isMotorCommandValid)
    {
        return -1;
    }
    while(false==robCtl->isInitialOk())
    {
//        qDebug()<<"servo on waitting";
        usleep(10000);
    }
    //安全判断
    if(1==allowServonCheck)
    {
        QVector<double> jointsPosition_c, jointsVel_c,jointAcc_c, jointTorque_c;
        QVector<double> jointsPosition_f, jointsVel_f,jointAcc_f, jointTorque_f;
        getJointCommand(jointsPosition_c, jointsVel_c,jointAcc_c, jointTorque_c);
        getFeedbackJoint(jointsPosition_f, jointsVel_f,jointAcc_f, jointTorque_f);
        for(int i=0;i<jointsPosition_c.size();i++)
        {
            double tmpDiff=fabs(jointsPosition_c[i]-jointsPosition_f[i]);
            if(tmpDiff>allowServonCheckDifference)
            {
                qDebug()<<"error---------------,servon error,反馈与命令值偏差到过允许，axisid="<<i
                       <<"tmpDiff="<<tmpDiff;
                addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5036,robotId,i,tmpDiff);
                return -1;
            }
        }

    }
    if(false==isAxisZeroOk() && true==allowSingleAxisJogFlag)
    {
        addMsg(ENUM_MSG_WARNING,"MotionServer","trajectory",5064,robotId);
        return -1;
    }
    resetAll_exceptStatus();
    pthread_mutex_lock(&trajMutex);//test
    if(true==robCtl->isServoOnRobot(robotId))
    {
        pthread_mutex_unlock(&trajMutex);//test
       return 2;
    }
    robCtl->servoOnRobot( robotId );
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}

int TrajectoryServer::servoOff_all()
{
//    qDebug()<<"TrajectoryServer::servoOff_all()";
    if(jointsNum<=0)
    {
        return -1;
    }
    pthread_mutex_lock(&trajMutex);//test
    robCtl->servoOffRobot( robotId );
    coupleManagement->disableAllCouple();
    pthread_mutex_unlock(&trajMutex);//test

    return 1;
}

bool TrajectoryServer::isServoOn()
{
    if(jointsNum<=0)
    {
        return true;
    }

    QVector<bool> flagReturn;
    pthread_mutex_lock(&trajMutex);//test
    robCtl->getServoOnStatus( robotId ,flagReturn);
    for(int i=0;i<flagReturn.size();i++)
    {
        bool tmpIsMustOn;
        tmpIsMustOn= isMustOnAxis(axisLogicalTypeVector[i]);
        if(false==flagReturn[i] && true==tmpIsMustOn)
        {
            pthread_mutex_unlock(&trajMutex);//test
            return false;
        }
    }
    pthread_mutex_unlock(&trajMutex);//test
    return true;

}

bool TrajectoryServer::isServoOn_all()
{
    if(jointsNum<=0)
    {
        return true;
    }
    pthread_mutex_lock(&trajMutex);//test
    bool tmpValue=robCtl->isServoOnRobot( robotId );
    pthread_mutex_unlock(&trajMutex);//test
    return tmpValue;
}

bool TrajectoryServer::isMustOnAxis(E_AXIS_LOGIC_TYPE axisLogicTypeIn)
{
    switch(axisLogicTypeIn)
    {
    case E_AXIS_LOGIC_TYPE_COMMON:
    {
        return true;
        break;
    }
    case E_AXIS_LOGIC_TYPE_EXT:
    {
        return true;
        break;
    }
    case E_AXIS_LOGIC_TYPE_AGV:
    {
        return true;
        break;
    }
    case E_AXIS_LOGIC_TYPE_VIRTUAL_ROBOT:
    {
        return false;
        break;
    }
    case E_AXIS_LOGIC_TYPE_VIRTUAL_PARALLEL:
    {
        return false;
        break;
    }
    case E_AXIS_LOGIC_TYPE_VIRTUAL_CAM:
    {
        return false;
        break;
    }
    case E_AXIS_LOGIC_TYPE_VIRTUAL_STAND_ALONE:
    {
        return false;
        break;
    }
    }
    return true;
}

int TrajectoryServer::servoOn(int axisId)
{
    if(jointsNum<=0)
    {
        return -1;
    }
    while(false==robCtl->isInitialOk())
    {
//        qDebug()<<"servo on waitting";
        usleep(10000);
    }

    //安全判断
    if(1==allowServonCheck)
    {
        QVector<double> jointsPosition_c, jointsVel_c,jointAcc_c, jointTorque_c;
        QVector<double> jointsPosition_f, jointsVel_f,jointAcc_f, jointTorque_f;
        getJointCommand(jointsPosition_c, jointsVel_c,jointAcc_c, jointTorque_c);
        getFeedbackJoint(jointsPosition_f, jointsVel_f,jointAcc_f, jointTorque_f);

            double tmpDiff=fabs(jointsPosition_c[axisId]-jointsPosition_f[axisId]);
            if(tmpDiff>allowServonCheckDifference)
            {
                qDebug()<<"error---------------,servon error,反馈与命令值偏差到过允许，axisid="<<axisId
                       <<"tmpDiff="<<tmpDiff;
                addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5036,robotId,axisId,tmpDiff);
                return -1;
            }

    }
    pthread_mutex_lock(&trajMutex);//test
    bool tmpValue=robCtl->isServoOnRobotAxis(robotId,axisId);
    if(true==tmpValue)
    {
       pthread_mutex_unlock(&trajMutex);//test
       return 2;
    }
    robCtl->servoOnRobotAxis( robotId,axisId );
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}

int TrajectoryServer::servoOff(int axisId)
{
    if(jointsNum<=0)
    {
        return -1;
    }

    pthread_mutex_lock(&trajMutex);//test
    bool tmpValue=robCtl->isServoOnRobotAxis(robotId,axisId);
    if(false==tmpValue)
    {
        pthread_mutex_unlock(&trajMutex);//test
       return 2;
    }
    robCtl->servoOffRobotAxis( robotId,axisId );
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}

bool TrajectoryServer::isServoOn(int axisId)
{
    if(jointsNum<=0)
    {
        return true;
    }
    pthread_mutex_lock(&trajMutex);//test
    bool tmpValue=robCtl->isServoOnRobotAxis(robotId,axisId);
    pthread_mutex_unlock(&trajMutex);//test
    return tmpValue;
}

int TrajectoryServer::quickStop()
{
    addMsg(ENUM_MSG_WARNING,"MotionServer","trajectory",5082,robotId);
    if(0)
    {
            qDebug()<<"TrajectoryServer::quickStop()";
    }

//    robCtl->servoOffRobot(robotId);
    robCtl->quickStopRobot(robotId );//调试，临时屏蔽
    pthread_mutex_lock(&trajMutex);//test
    coupleManagement->disableAllCouple();
    pthread_mutex_unlock(&trajMutex);//test

    if(1==roboParameter->isAgvMotionOn)
    {
        magneticTrack->quickStop();
        virtualLineControl->quickStop();
    }
    if(1==roboParameter->isArmMotionOn)
    {
        //连续调用２次会导致立刻停止。
        resetAll_exceptStatus();
    }
    setBasicStatus(ENUM_TRAJECTORY_CEASED,9990001);

    switch(motionModel)
    {
    case ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS:
    {

        break;
    }
    case ENUM_MOTION_MODEL_HOMING:
    {
        haltGoZero_forUser();
        return 1;
        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK:
    {
        magneticTrack->stop();
        return 1;
        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK:
    {

        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_JOG:
    {
        vehicleJogControl->stop();
        return 1;
        break;
    }
    }

    return 1;
}
int TrajectoryServer::initialServoParameter(QVector<JointAttribute> jointAttributeVectorIn)
{
    if(jointAttributeVectorIn.size()>0)
    {
        for(int i=0;i<jointsNum;i++)
        { 
            //dc 有问题，导致偶尔卡，所有要设置30ms的力矩滤波
//            robCtl->setMotorTorqueFilterTime(robotId,i,
//                                            jointAttributeVectorIn[i].motorTorqueFilterTime);
            //限制电机最大力矩
            robCtl->setMotorTorqueLimit(robotId,i,
                   jointAttributeVectorIn[i].motorTorqueLimitPositive,
                   jointAttributeVectorIn[i].motorTorqueLimitNegative);
        }

    }

    #ifndef MOTION_SERVER_UNIT_TEST
    //测试SDO读写
    QVector<ULONG64> proportionForServo;
    QVector<ULONG64> integralForServo;
    QVector<ULONG64> kpp;
    QVector<ULONG64> positionSmooth;
    proportionForServo.resize(jointsNum);
    integralForServo.resize(jointsNum);
    kpp.resize(jointsNum);
    positionSmooth.resize(jointsNum);
    if (robotType != ENUM_7AXIS)
    {
        return -1;
    }
    LONG64 dataReturn;
    int returnLen = 2;
//    proportionForServo[0] = 200;
//    integralForServo[0] = 350;
//    kpp[0] = 30;
//    positionSmooth[0] = 5;
//    proportionForServo[1] = 250;
//    integralForServo[1] = 500;
//    kpp[1] = 10;
//    positionSmooth[1] = 5;
//    proportionForServo[2] = 200;
//    integralForServo[2] = 350;
//    kpp[2] = 30;
//    positionSmooth[2] = 5;
//    proportionForServo[3] = 200;
//    integralForServo[3] = 350;
//    kpp[3] = 30;
//    positionSmooth[3] = 5;
//    proportionForServo[4] = 200;
//    integralForServo[4] = 350;
//    kpp[4] = 30;
//    positionSmooth[4] = 5;
//    proportionForServo[5] = 200;
//    integralForServo[5] = 350;
//    kpp[5] = 30;
//    positionSmooth[5] = 5;
//    proportionForServo[6] = 200;
//    integralForServo[6] = 350;
//    kpp[6] = 30;
//    positionSmooth[6] = 5;

   // -----------------------------------
    proportionForServo[0] = 100;
    integralForServo[0] = 550;
    kpp[0] = 10;
    positionSmooth[0] = 100;
    proportionForServo[1] = 100;
    integralForServo[1] = 550;
    kpp[1] = 10;
    positionSmooth[1] = 100;
    proportionForServo[2] = 100;
    integralForServo[2] = 550;
    kpp[2] = 10;
    positionSmooth[2] = 100;
    proportionForServo[3] = 100;
    integralForServo[3] = 550;
    kpp[3] = 10;
    positionSmooth[3] = 100;
    proportionForServo[4] = 100;
    integralForServo[4] = 550;
    kpp[4] = 10;
    positionSmooth[4] = 100;
    proportionForServo[5] = 100;
    integralForServo[5] = 550;
    kpp[5] = 10;
    positionSmooth[5] = 100;
    proportionForServo[6] = 100;
    integralForServo[6] = 550;
    kpp[6] = 10;
    positionSmooth[6] = 100;
    for (int i = 0; i < jointsNum; i++)
    {
        robCtl->setSdo(robotId, i, 0x3507, 0, 0, returnLen);
        robCtl->setSdo(robotId, i, 0x3504, 0, proportionForServo[i], returnLen);
        robCtl->setSdo(robotId, i, 0x3505, 0, integralForServo[i], returnLen);
        robCtl->setSdo(robotId, i, 0x3500, 0, kpp[i], returnLen);
        robCtl->setSdo(robotId, i, 0x3503, 0, positionSmooth[i], returnLen);
    }
    for (int i = 0; i < jointsNum; i++)
    {
        robCtl->getSdo(robotId, i, 0x3504, 0, dataReturn, returnLen);
        qDebug() << "----------------///////////axis:" << i << "-------proportionForServo-------0x3504:" << dataReturn;
        robCtl->getSdo(robotId, i, 0x3505, 0, dataReturn, returnLen);
        qDebug() << "---------------///////////axis:" << i << "---------integralForServo-----0x3505:"  << dataReturn;
    }

    #endif
    return 1;
}

int TrajectoryServer::initialServoParameter()
{
    return initialServoParameter(roboParameter->jointAttributeVector);
}


int TrajectoryServer::setMotorFeedbackDataVectorFromEthercatNoLock()
{

    //从EtherCAT读关节数据，包括位置、速度和力矩
    if(ENUM_TRAJECTORY_INITIAL==basicStatus)
    {
      return -1;
    }

    QVector<MotorData> motorFeedback;
    motorFeedback.resize(jointsNum);
    if(  eMasterOP==getMasterState())
    {
        QVector<ServoState> pData = robCtl->getServoDataRobot(robotId );
        if(jointsNum != pData.size())
        {
            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5014,robotId,jointsNum,pData.size());

            if(debugFlag)
            {
                qDebug()<<"setMotorFeedbackDataVectorFromEthercat_UpdateCurrentJointPointCoordinatePoint error,读取的关节数量与初始化关节数量不一致";
            }
            return -1;
        }

        for(int i=0;i<jointsNum; i++)
        {
            motorFeedback[i].position = pData[i].position;//位置
            motorFeedback[i].speed = pData[i].speed;//速度
            motorFeedback[i].torque=pData[i].torque;

        }
    }

    //setMotorFeedbackDataVector
    if(motorFeedback.size()< jointsNum )
    {
        qDebug()<<"TrajectoryServer::setMotorFeedbackDataVector(QVector<MotorData> motorFeedbackIn) error";
        return -1;
    }
    axisGroup->setMotorFeedbackDataVector(motorFeedback);

    return 1;
}

int TrajectoryServer::setMotorFeedbackDataVectorFromEthercat_UpdateCurrentJointPointCoordinatePoint(const QVector<MotorData> &motorFeedback)
{
    #ifndef MOTION_SERVER_UNIT_TEST
    //从EtherCAT读关节数据，包括位置、速度和力矩
    if(ENUM_TRAJECTORY_INITIAL==basicStatus)
    {
      return -1;
    }
    motionDebugInfo.realThreadStage=E_REAL_THREAD_SET_MOTOR_FEEDBACK;

    setMotorFeedbackDataVector(motorFeedback);
    calculateCurrentPoint();//testhualei
//    setFrameFeedback();

#endif
    return 1;
}

E_ECAT_MASTER_STATE TrajectoryServer::getMasterState()
{
    return robCtl->getMasterState();
}

double TrajectoryServer::getDeltaTime()
{
    return deltaTime;
}


int TrajectoryServer::trajectoryRun()
{
    if(isAxisZeroOk()==false && true==allowSingleAxisJogFlag && jointsNum>0)
    {
        if(debugFlag)
        {
            qDebug()<< "TrajectoryServer::run()====isAxisZeroOk()==false";
        }

        addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 5037, robotId);
    }

    if(coordinateManager->getInitialStatus()==false && false==allowSingleAxisJogFlag
            && jointsNum>0)
    {
        if(debugFlag)
        {
            qDebug()<< "TrajectoryServer::run()====coordinateManager->isInitialOk()==false";
        }

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5026, robotId);
        resetAll(100);
        return -1;
    }



    if(isAxisZeroOk()==false && false==allowSingleAxisJogFlag && jointsNum>0)
    {
        if(debugFlag)
        {
            qDebug()<< "TrajectoryServer::run()====isAxisZeroOk()==false";
        }

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5023, robotId);
        resetAll(101);
        return -1;
    }
    if(isServoOn()==false && jointsNum>0)
    {
        if(debugFlag)
        {
            qDebug()<< "TrajectoryServer::run()====isServoOn()==false";
        }

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5024, robotId);
        resetAll(102);
        return -1;
    }


    if(false==isAbsoulteMotorBatteryOk() && jointsNum>0)
    {
        if(debugFlag)
        {
            qDebug()<< "TrajectoryServer::run()===, false==isAbsoulteMotorBatteryOk(robotId)";
        }
        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5023, robotId);
        resetAll(103);
        return false;
    }

    if(ENUM_TRAJECTORY_CEASED!=basicStatus && ENUM_TRAJECTORY_STOPPED!=basicStatus)
    {
        if(1)
        {
            qDebug()<< "TrajectoryServer::run()===, ENUM_TRAJECTORY_CEASED!=basicStatus && ENUM_TRAJECTORY_STOPPED!=basicStatus"
                    <<basicStatus;
        }
//        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5023, robotId);
    }

    beltTrackManager->setWork(true);


    if(ENUM_TRAJECTORY_TOSTOP ==basicStatus || ENUM_TRAJECTORY_STOPPING ==basicStatus ||
                                                    ENUM_TRAJECTORY_STOPPED  ==basicStatus)
    {
        carvingModule->emcTpClear();

    }
    carvingModule->emcTpResume();

//    basicStatus=ENUM_TRAJECTORY_RUN;重复了


    qDebug()<<"TrajectoryServer::trajectoryRun,,,,,,,,,,,,,,,,,,";
    return 1;
}

int TrajectoryServer::startOneModel(MC_MOTION_MODEL modelIn)
{
    qDebug()<<"TrajectoryServer::startOneModel"<<modelIn;
    if(1==roboParameter->isAgvMotionOn)
    {
        virtualLineControl->startRun();
        magneticTrack->startRun();
    }
    if(1==roboParameter->isArmMotionOn)
    {
        trajectoryRun();
    }
    switch(modelIn)
    {
    case ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS:
    {

        break;
    }
    case ENUM_MOTION_MODEL_HOMING:
    {
        //
        return 1;
        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK:
    {
        magneticTrack->startRun();
        return 1;
        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK:
    {

        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_JOG:
    {
        vehicleJogControl->startRun();
        return 1;
        break;
    }
    }

    return 1;
}

int TrajectoryServer::allowSingleAxisJog(bool enable)
{
    qDebug()<<"TrajectoryServer::allowSingleAxisJog, enable="<<enable;
    allowSingleAxisJogFlag=enable;
    return 1;
}

int TrajectoryServer::allStartRun()
{
    qDebug()<<"TrajectoryServer::allStartRun,,,,,,,,,,,,,,,,,,";

    startOneModel(motionModel);

    setBasicStatus(ENUM_TRAJECTORY_RUN,9990002);
    return 1;
}

int TrajectoryServer::virtualLineRun()
{
    if(1==roboParameter->isAgvMotionOn)
    {
        virtualLineControl->startRun();
        magneticTrack->startRun();
    }
    return 1;
}


int TrajectoryServer::allCease()
{
#ifdef D_USE_ROS
    if(1==roboParameter->isAgvMotionOn)
    {
        basicStatus=ENUM_TRAJECTORY_CEASED;//会导致机械臂立刻停止
        magneticTrack->cease();
        virtualLineControl->cease();

    }
#endif

    if(1==roboParameter->isArmMotionOn)
    {
        carvingModule->emcTpPause();
        ceaseAxisGroup();

        if(ENUM_TRAJECTORY_RUN==basicStatus)
        {
            //连续调用２次会导致立刻停止。保证只调用１次
            setBasicStatus(ENUM_TRAJECTORY_TOCEASE,9990003);

            double tmpStopTime=getAxisGroupStopTime();
            if(tmpStopTime>D_STOP_TIME_MAX)
            {
                tmpStopTime=D_STOP_TIME_MAX;
                qDebug()<<"warn,,,,,tmpStopTime>D_STOP_TIME_MAX,,,,,,,,,,,tmpStopTime"<<tmpStopTime;
            }
            setMianControlRatio(0);
            setTimeAxisScale(0,roboParameter->arm_CeaseTime*tmpStopTime);
            qDebug()<<"allCease:roboParameter->arm_CeaseTime"<<roboParameter->arm_CeaseTime
                   <<"getAxisGroupStopTime"<<tmpStopTime;

            setBasicStatus(ENUM_TRAJECTORY_CEASING,9990004);
        }

        beltTrackManager->cease();


    }
    switch(motionModel)
    {
    case ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS:
    {

        break;
    }
    case ENUM_MOTION_MODEL_HOMING:
    {
        haltGoZero_forUser();
        return 1;
        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK:
    {
        setBasicStatus(ENUM_TRAJECTORY_CEASED,9990005);
        magneticTrack->cease();
        return 1;
        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK:
    {

        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_JOG:
    {
        vehicleJogControl->stop();
        return 1;
        break;
    }
    }

    return 1;

}

int TrajectoryServer::virtualLineCease()
{
    if(1==roboParameter->isAgvMotionOn)
    {
//        basicStatus=ENUM_TRAJECTORY_CEASED;
        magneticTrack->cease();
        virtualLineControl->cease();

    }
    return 1;
}


int TrajectoryServer::allStop()
{
    qDebug()<<"TrajectoryServer::allStop";
#ifdef D_USE_ROS
    if(1==roboParameter->isAgvMotionOn)
    {
        resetAll();
        basicStatus=ENUM_TRAJECTORY_STOPPED;
        magneticTrack->stop();
        magneticTrack->resetAll();
        vehicleJogControl->stop();
        virtualLineControl->stop();
    }
#endif

    if(1==roboParameter->isArmMotionOn)
    {
        carvingModule->emcTpPause();
        ceaseAxisGroup();
        if(ENUM_TRAJECTORY_RUN!=basicStatus)
        {
            resetAll(104);
        }
        else
        {
//            beltTrackManager->setWork(false);
            setBasicStatus(ENUM_TRAJECTORY_TOSTOP,9990006);
            double tmpStopTime=getAxisGroupStopTime();
            setMianControlRatio(0);
            setTimeAxisScale(0,roboParameter->arm_StopTime*tmpStopTime);
            qDebug()<<"allCease:roboParameter->allStop"<<roboParameter->arm_StopTime
                   <<"getAxisGroupStopTime"<<tmpStopTime;

            setBasicStatus(ENUM_TRAJECTORY_STOPPING,9990007);

        }
    }

    switch(motionModel)
    {
    case ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS:
    {

        break;
    }
    case ENUM_MOTION_MODEL_HOMING:
    {
        haltGoZero_forUser();
        restorNormalOperationMode();
        setMotionServerModel(ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS);
        return 1;
        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_JOG:
    {
        magneticTrack->stop();
        vehicleJogControl->stop();
        return 1;
        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL:
    {
        vehicleJogControl->stop();
        return 1;
        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK:
    {
        setBasicStatus(ENUM_TRAJECTORY_STOPPED,9990008);
        magneticTrack->stop();
        magneticTrack->resetAll();
        vehicleJogControl->stop();
        return 1;
        break;
    }
    case ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK:
    {

        break;
    }
    default:
    {

    }
    }

    return 1;

}

int TrajectoryServer::resetAll(int id)
{
    qDebug()<<"TrajectoryServer::resetAll id"<<id;

    setIsInCarvingMode(false);
    currentLine=ENUM_LINE_INITIAL;
    setBasicStatus(ENUM_TRAJECTORY_STOPPED,9990009);
    resetAll_exceptStatus();
    beltTrackManager->reset();//不能放在下面，否则运动报错后恢复，会导致模块状态被重置。
    carvingModule->resetAll();
    return 1;
}

int TrajectoryServer::resetAll_exceptStatus()
{
//    qDebug()<<"TrajectoryServer::resetAll_exceptStatus";
    pthread_mutex_lock(&trajMutex);//test
    commandQueue.clear();
    axisGroup->resetAll();
    coupleManagement->resetCoupleOffset();
    gantryControl->resetAll();

    setPositionFrameCommand_byJointCommand();

    pthread_mutex_unlock(&trajMutex);//test
    synchronizeMotorFeedbackToCalculatePosition();
//    basicStatus=ENUM_TRAJECTORY_STOPPED;
    resetTrajTime();
    allowSingleAxisJogFlag=false;
    isMoveToPoint=false;
    exceedTimeCount=0;
    laserLoseRescueFlag=0;

    adjustSpeedRatio=1;
    slowdownTickcount=0;
    goZeroEnableFlag=0;


    return 1;
}

int TrajectoryServer::resetAll_and_virtualLine()
{
    if(1==roboParameter->isAgvMotionOn)
    {
        //下面会导致旧的轨迹被清空。这样可能有风险，如果用户手动把ＡＧＶ移动到了别处，然后继续运行，会猛冲。
        updateVirtualLinePositionFeedback();
        virtualLineControl->resetAll();

    }
    if(1==roboParameter->isArmMotionOn)
    {
        resetAll(105);
    }

    return 1;
}

int TrajectoryServer::resetAll_and_virtualLine_no_clear_queue()
{
    if(1==roboParameter->isAgvMotionOn)
    {
        //下面会导致旧的轨迹被清空。这样可能有风险，如果用户手动把ＡＧＶ移动到了别处，然后继续运行，会猛冲。
        updateVirtualLinePositionFeedback();
//        virtualLineControl->resetAll();

    }
    if(1==roboParameter->isArmMotionOn)
    {
        resetAll(106);
    }

    return 1;
}

//int TrajectoryServer::stoppedStatusUpdate()
//{
//    axisGroup->syncCommandAndFeedback();
//    return 1;
//}

int TrajectoryServer::clearTraj(bool isSyncFeedbackPos)
{
    pthread_mutex_lock(&trajMutex);//test
    commandQueue.clear();

    axisGroup->clearTraj(isSyncFeedbackPos);
    pthread_mutex_unlock(&trajMutex);//test
    resetTrajTime();

    if(isSyncFeedbackPos)
    {
        synchronizeMotorFeedbackToCalculatePosition();
    }
    else
    {
        syncAxisPoseCommandToCarvingModule();
    }

    adjustSpeedRatio=1;
    carvingModule->emcTpClear();

    return 1;
}



int TrajectoryServer::clearTraj_byAxisId(int axisIndex,bool isSyncFeedbackPos)
{
    pthread_mutex_lock(&trajMutex);//test
    commandQueue.clear();
    axisGroup->clearTraj_byAxisId(axisIndex,isSyncFeedbackPos);
    pthread_mutex_unlock(&trajMutex);//test
    resetTrajTime();
    synchronizeMotorFeedbackToCalculatePosition(axisIndex);
    return 1;
}


int TrajectoryServer::setTimeAxisScale(double ratio, double timeDuration, int timeAxisIndex)
{

    pthread_mutex_lock(&trajMutex);//test
    axisGroup->setTimeAxisScale(ratio,timeDuration,timeAxisIndex);
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}
int TrajectoryServer::setTimeAxisScale(double ratio, double timeDuration)
{
    timeAxisParameter.startRatio=ratio;
    timeAxisParameter.startDuration=timeDuration;
    //下面的语句会导致２次掉用setRatio的时候，立刻停止
    oldTimeAxisRatio=newTimeAxisRatio;
    newTimeAxisRatio=ratio;
    if(newTimeAxisRatio>oldTimeAxisRatio)
    {
        timeAxisStatus=ENUM_TIME_AXIS_ASCEND;
    }
    else if(newTimeAxisRatio==oldTimeAxisRatio)
    {
        timeAxisStatus=ENUM_TIME_AXIS_NORMAL;
    }
    else
    {
        timeAxisStatus=ENUM_TIME_AXIS_DESCEND;
    }
    pthread_mutex_lock(&trajMutex);//test
    axisGroup->setAllTimeAxisScale(ratio,timeDuration);
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}


int TrajectoryServer::getJointCommand(QVector<double> &jointsPositionOut, QVector<double> &jointsVelOut,
                                      QVector<double> &jointsAccOut, QVector<double> &jointsTorqueOut)
{
    pthread_mutex_lock(&trajMutex);//test
    axisGroup->getJointCommand(jointsPositionOut,jointsVelOut,jointsAccOut,jointsTorqueOut);
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}

int TrajectoryServer::getJointFeedback(QVector<double> &jointsPositionOut, QVector<double> &jointsVelOut,
                                       QVector<double> &jointsAccOut, QVector<double> &jointsTorqueOut)
{
    pthread_mutex_lock(&trajMutex);//test
    axisGroup->getJointFeedback(jointsPositionOut,jointsVelOut,jointsAccOut,jointsTorqueOut);
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}

bool TrajectoryServer::isAbsoulteMotorBatteryOk()
{
    for(int i=0;i<roboParameter->jointNum;i++)
    {
        if(true==axisGroup->isAbsoluteMotor(i))
        {
            if(EN_BATTERY_STATUS_LOOSE_VOLTAGE==robCtl->isRobotMotorBatteryOk(robotId,i))
            {
                axisLosePower(i);
                return false;
            }
        }
    }

    return true;
}

int TrajectoryServer::testRoutine()
{
    //计算圆心和向量
    std::vector<double> centerOut, normalOut,tmpStartPos,tmpMiddlePos,tmpEndPos;
    //case1
    tmpStartPos.push_back(100);
    tmpStartPos.push_back(0);
    tmpStartPos.push_back(0);

    tmpMiddlePos.push_back(0);
    tmpMiddlePos.push_back(100);
    tmpMiddlePos.push_back(0);

    tmpEndPos.push_back(-70);
    tmpEndPos.push_back(70);
    tmpEndPos.push_back(0);

    int transResult=transCircleInfo(tmpStartPos,tmpMiddlePos
                    ,tmpEndPos,centerOut, normalOut);
    if(1!=transResult)
    {
        qDebug()<<"error,,,,transCircleInfo";
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5090,robotId);
        return -1;
    }

    transResult=transCircleInfo(tmpStartPos,tmpEndPos,tmpMiddlePos
                        ,centerOut, normalOut);
    if(1!=transResult)
    {
        qDebug()<<"error,,,,transCircleInfo";
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5090,robotId);
        return -1;
    }

    //case2
    tmpStartPos.clear();
    tmpMiddlePos.clear();
    tmpEndPos.clear();

    tmpStartPos.push_back(100);
    tmpStartPos.push_back(0);
    tmpStartPos.push_back(0);

    tmpMiddlePos.push_back(0);
    tmpMiddlePos.push_back(-100);
    tmpMiddlePos.push_back(0);

    tmpEndPos.push_back(-70);
    tmpEndPos.push_back(-70);
    tmpEndPos.push_back(0);

    transResult=transCircleInfo(tmpStartPos,tmpMiddlePos
                    ,tmpEndPos,centerOut, normalOut);
    if(1!=transResult)
    {
        qDebug()<<"error,,,,transCircleInfo";
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5090,robotId);
        return -1;
    }

    transResult=transCircleInfo(tmpStartPos,tmpEndPos,tmpMiddlePos
                        ,centerOut, normalOut);
    if(1!=transResult)
    {
        qDebug()<<"error,,,,transCircleInfo";
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5090,robotId);
        return -1;
    }


    //case3
    tmpStartPos.clear();
    tmpMiddlePos.clear();
    tmpEndPos.clear();

    tmpStartPos.push_back(100);
    tmpStartPos.push_back(0);
    tmpStartPos.push_back(0);

    tmpMiddlePos.push_back(-70);
    tmpMiddlePos.push_back(70);
    tmpMiddlePos.push_back(0);

    tmpEndPos.push_back(-70);
    tmpEndPos.push_back(-70);
    tmpEndPos.push_back(0);

    transResult=transCircleInfo(tmpStartPos,tmpMiddlePos
                    ,tmpEndPos,centerOut, normalOut);
    if(1!=transResult)
    {
        qDebug()<<"error,,,,transCircleInfo";
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5090,robotId);
        return -1;
    }

    transResult=transCircleInfo(tmpStartPos,tmpEndPos,tmpMiddlePos
                    ,centerOut, normalOut);
    if(1!=transResult)
    {
        qDebug()<<"error,,,,transCircleInfo";
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5090,robotId);
        return -1;
    }
    //case4
    tmpStartPos.clear();
    tmpMiddlePos.clear();
    tmpEndPos.clear();

    tmpStartPos.push_back(-517.704);
    tmpStartPos.push_back(260);
    tmpStartPos.push_back(150);

    tmpMiddlePos.push_back(-411);
    tmpMiddlePos.push_back(290);
    tmpMiddlePos.push_back(150);

    tmpEndPos.push_back(-338);
    tmpEndPos.push_back(-330);
    tmpEndPos.push_back(150);

    transResult=transCircleInfo(tmpStartPos,tmpMiddlePos
                    ,tmpEndPos,centerOut, normalOut);
    if(1!=transResult)
    {
        qDebug()<<"error,,,,transCircleInfo";
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5090,robotId);
        return -1;
    }

    transResult=transCircleInfo(tmpStartPos,tmpEndPos,tmpMiddlePos
                        ,centerOut, normalOut);
        if(1!=transResult)
        {
            qDebug()<<"error,,,,transCircleInfo";
            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5090,robotId);
            return -1;
        }

                return 1;
}

int TrajectoryServer::pointProcessForTraj(int lineNumber,PointPro &targetPoint)
{
    if( (1==targetPoint.jointFlag)&&//笛卡尔点
            ((targetPoint.ufValue != coordinateManager->getCurrentUserIndex()) ||//用户坐标系编号不一致
             (targetPoint.utValue != coordinateManager->getCurrentToolIndex())) )//工具坐标系编号不一致
    {
        if(debugFlag)
        {
            qDebug()<< "McControllerInterpreter::transCommandQueueToCarvingModule====笛卡尔点坐标系编号不一致"\
                    << targetPoint.ufValue<<", "<<targetPoint.utValue;
        }

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5019, robotId, lineNumber);
        return -1;
    }

    //点统一转化为迪卡尔点
    if(1!=pointCoordinateToCartesian(targetPoint,targetPoint))
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5076,robotId);
        return -1;
    }

    //点多解信息匹配检查
    vector<double> initialJointPosition;
    initialJointPosition.resize(innerJointsNum);
    QVector<double> jointsPositionOut;
    pthread_mutex_lock(&trajMutex);//test
    axisGroup->getJointCommand(jointsPositionOut);
    for(int i=0;i<innerJointsNum;i++)
    {
        initialJointPosition[i]=jointsPositionOut[i];
    }
    int futResutl=coordinateManager->isPointFutCorrect(targetPoint, initialJointPosition);
    pthread_mutex_unlock(&trajMutex);//test
//    //     if(1!=futResutl)//ur机器人会有问题。
//    //     {
//    //         qDebug()<<"error,processLineInstruction coordinateManager->isPointFutCorrect";
//    //         addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5020,robotId);
//    //         return -1;
//    //     }
    return 1;
}

int TrajectoryServer::transCircleInfo(std::vector<double> startPose,std::vector<double> middlePose,
                                      std::vector<double> endPose,
                                      std::vector<double> &centerOut, std::vector<double> &normalOut)
{
    qDebug()<<"transCircleInfo"<<QVector<double>::fromStdVector(startPose)<<QVector<double>::fromStdVector(middlePose)
              <<QVector<double>::fromStdVector(endPose);
    Vector positionFrameCommand_planed = Vector (startPose[0],startPose[1],startPose[2]);
    Vector positionFrame_middle = Vector (middlePose[0],middlePose[1],middlePose[2]);
    Vector positionFrame_end = Vector (endPose[0],endPose[1],endPose[2]);

    double eps = 1e-6;//20170222华磊修改,原为1E-7

    Vector tmp1( positionFrameCommand_planed-positionFrame_middle );//第一个向量，从第一个点到初始点
    Vector tmp2(positionFrame_end - positionFrame_middle);//第二个向量，从第一个点到第二个点
    Vector tmp3(positionFrame_end - positionFrameCommand_planed);
    double l1 = tmp1.Normalize();//单位化第一个向量，并求长度
    double l2 = tmp2.Normalize();//单位化第二个向量，并求长度
    double l3 = tmp3.Normalize();//单位化第二个向量，并求长度
    double angle = acos( dot(tmp1,tmp2) );//中间点的夹角

    if( ( l1 < eps ) || (l2 < eps) || ((M_PI-angle) < eps) || (angle < eps) )//有些点重合或者在同一直线上
    {
        if( angle < eps )
        {
            qDebug()<<"circleMove error,angle < eps ";
        }
        else
        {
            qDebug()<<"circleMove error,angle >=eps ";
        }
        qDebug()<<"( l1 < eps ) || (l2 < eps) || ((M_PI-angle) < eps) || (angle < eps)  ";
        return -1;
    }
    Vector tmpCenterNormal = tmp1 * tmp2; //圆心，初始化值为圆的法向量
    Vector center=tmpCenterNormal;

    Matrix3d A;
    Vector3d b,c;
    A << center.data[0], center.data[1], center.data[2],
            tmp1.data[0], tmp1.data[1], tmp1.data[2],
            tmp2.data[0], tmp2.data[1], tmp2.data[2];
    b<< dot(center, positionFrameCommand_planed),
            dot(positionFrameCommand_planed+positionFrame_middle,
                positionFrameCommand_planed-positionFrame_middle)/
            (2.0*l1), dot(positionFrame_end+positionFrame_middle,
                          positionFrame_end-positionFrame_middle)/(2.0*l2);
    c =A.inverse()*b;
    centerOut.resize(3);
    centerOut[0] = c(0);
    centerOut[1] = c(1);
    centerOut[2] = c(2);


    Vector r1(positionFrame_middle - center);
    double radius = r1.Norm(); //半径 kdlerror

    double rad1= 2.0*asin(l1/2.0/radius);//第一段弧度
    double rad3 = 2.0*asin(l3/2.0/radius);


    Vector startVec1(positionFrameCommand_planed - center);

    if(rad3>rad1)
    {
        Vector endVec_mi(positionFrame_middle - center);
        tmpCenterNormal=startVec1*endVec_mi;
//        qDebug()<<"rad3>rad1";
    }
    else
    {
        Vector endVec_en(positionFrame_end - center);
        tmpCenterNormal=endVec_en*startVec1;
//        qDebug()<<"rad3<rad1";
    }
    tmpCenterNormal.Normalize();
    tmpCenterNormal=-tmpCenterNormal;

//    qDebug()<<" rad1"<<rad1<<"rad3"<<rad3<<"radius"<<radius
//           <<"center"<<centerOut[0]<<centerOut[1]<<centerOut[2]
//             <<"normal"<<tmpCenterNormal[0]<<tmpCenterNormal[1]<<tmpCenterNormal[2];


    normalOut.resize(3);
    normalOut[0]=tmpCenterNormal.data[0];
    normalOut[1]=tmpCenterNormal.data[1];
    normalOut[2]=tmpCenterNormal.data[2];//圆心有ｍｍ误差，向量也不是垂直的有０．３


    return 1;


}

int TrajectoryServer::transferRPY(std::vector<double> poseIn, std::vector<double> &poseOut)
{
    poseOut=poseIn;
    poseOut.push_back(0);
    poseOut.push_back(0);
    poseOut.push_back(0);
    coordinateManager->getBestPositionFromAngle(poseOut[3],tpBeforePose[3],poseOut[3]);
    coordinateManager->getBestPositionFromAngle(poseOut[4],tpBeforePose[4],poseOut[4]);
    coordinateManager->getBestPositionFromAngle(poseOut[5],tpBeforePose[5],poseOut[5]);
    int tmpSize=poseOut.size();
    for(int i=0;i<tmpSize;i++)
    {
        tpBeforePose[i]=poseOut[i];
    }
//    qDebug()<<"---------------transferRPY poseIn"<<QVector<double>::fromStdVector(poseIn)<<"poseOut"<<QVector<double>::fromStdVector(poseOut)
//           <<"tpBeforePose"<<tpBeforePose[3]<<tpBeforePose[4]<<tpBeforePose[5];
    return 1;
}

int TrajectoryServer::transRPY2Quan(std::vector<double> poseIn, std::vector<double> &poseOut)
{
    poseOut.resize(9);
    poseOut[0]=poseIn[0];
    poseOut[1]=poseIn[1];
    poseOut[2]=poseIn[2];
    poseOut[7]=0;
    poseOut[8]=0;
    Rotation tmpOrient=Rotation::RPY(poseIn[3]/180.0*M_PI,poseIn[4]/180.0*M_PI,poseIn[5]/180.0*M_PI);
    tmpOrient.GetQuaternion(poseOut[3],poseOut[4],poseOut[5],poseOut[6]);
    return 1;
}

bool TrajectoryServer::isCoupleControlFollowAxisWork(int followAxisId)
{

}

bool TrajectoryServer::isGantryControlFollowAxisWork(int followAxisId)
{

}

int TrajectoryServer::calibrateCheck()
{
    if(calibrateCheckFinished)
    {
        return 0;
    }
    QVector<ZeroReturnParameter> zeroInfo;
    pthread_mutex_lock(&trajMutex);//test
    axisGroup->getZeroReturnParameter(zeroInfo);
    pthread_mutex_unlock(&trajMutex);//test

    //是否需要电阻编码器校准。
    int needAxisCount=0;
    for(int i=0;i<zeroInfo.size();i++)
    {
        if(0<=zeroInfo[i].aiEncoderIndex)
        {
            needAxisCount++;
        }
    }
    if(0==needAxisCount)
    {
        return 0;
    }

    //等待设备初始化
    double tmpAiValue;
    int tmpTryCount=10;
    do
    {
        sleep(1);
        tmpAiValue=ioController->getAi(0);
        tmpTryCount--;
    }while(tmpAiValue<0.1 && tmpTryCount>0);
    if(tmpAiValue<0.1)
    {
        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5063, robotId);
        return -1;
    }

    //校准
    pthread_mutex_lock(&trajMutex);//test
    for(int i=0;i<zeroInfo.size();i++)
    {
        if(0<=zeroInfo[i].aiEncoderIndex)
        {
            axisGroup->calibrateJointByAiEncoder(i,ioController->getAi(zeroInfo[i].aiEncoderIndex));
        }
    }

    calibrateReset();
    pthread_mutex_unlock(&trajMutex);//test
    synchronizeMotorFeedbackToCalculatePosition();
    calibrateCheckFinished=true;


}

bool TrajectoryServer::isLineFinished(double currentLineTimeDurationIn)
{
    double lineTimeCount=axisGroup->getAccumulatedTime();
    currentLineRemainingTime=currentLineTimeDurationIn-lineTimeCount;
    //主要是为了和多段平滑兼容，有暂停后再次运行不加新运动指令的风险。
//    if(currentLineRemainingTime<0.00000001 || true==isInFineMoving())
//    if(currentLineRemainingTime<0.00000001 || 1==coordinateTrajectory->isVelProfileEmpty())
    if(currentLineRemainingTime<0.00000001 || 1==isTrajectoryEmpty_nolock())
    {
        if(D_LINE_DEBUG)
        {
            qDebug()<<"D_LINE_DEBUG1 currentLineRemainingTime"<<currentLineRemainingTime
                   <<"lineTimeCount"<<lineTimeCount<<"currentLineTimeDurationIn"<<currentLineTimeDurationIn
                  <<isInFineMoving();
        }

        return true;
    }
    return false;
}

int TrajectoryServer::setServoOnStatus(const QVector<bool> &servoOnStatus)
{
    pthread_mutex_lock(&trajMutex);//test
    int tmpKey=axisGroup->setServoOnStatus(servoOnStatus);
    pthread_mutex_unlock(&trajMutex);//test
    return tmpKey;
}

int TrajectoryServer::axisFeedbackCheck()
{
    pthread_mutex_lock(&trajMutex);//test
    int tmpKey=axisGroup->axisFeedbackCheck();
    pthread_mutex_unlock(&trajMutex);//test
    if(1!=tmpKey)
    {
        quickStop();
        addMsg(ENUM_MSG_ERROR,"MotionServer","TrajectoryServer",5058,robotId);
        if(debugFlag)
        {
            qDebug()<< "axisFeedbackCheck quickstop";
        }
    }
    return tmpKey;
}

int TrajectoryServer::axisPositionCommandLimitApproachCheck()
{
//    if(ENUM_CONTROL_T1!=controlType)
//    {
//        return 0;
//    }

    //快到达，而且接近极限位置时暂停。
    pthread_mutex_lock(&trajMutex);//test
    int tmpKey=axisGroup->isPositionCommandApprochLimit();
    bool relativeDistanceTriged=coupleManagement->isRelativeDistanceApproachTrigged();
    pthread_mutex_unlock(&trajMutex);//test
    if(0!=tmpKey )//&&  ENUM_TRAJECTORY_RUN==basicStatus
    {
        allCease();
//        addMsg(ENUM_MSG_ERROR,"MotionServer","TrajectoryServer",5058,robotId);
        if(debugFlag)
        {
            qDebug()<< "axisPositionCommandLimitApproachCheck cease";
        }
    }

    //耦合距离监视
    if(true==relativeDistanceTriged )//&&  ENUM_TRAJECTORY_RUN==basicStatus
    {
        allCease();
        if(debugFlag)
        {
            qDebug()<< "axisPositionCommandLimitApproachCheck cease";
        }
    }
    return tmpKey;
}



int TrajectoryServer::laserLoseRescueLogic()
{
    #ifdef D_USE_ROS
    E_VEHICLE_POSITION_TYPE positonType;
    vehiclePositionManage->getVehiclePositionType(positonType);
    QVector<bool> isLaseLoseListOut;
    m_rosApi->getLaserLoseStatus(isLaseLoseListOut);
    bool tfOk=m_rosApi->isTfOkStatus();
    if(0==laserLoseRescueFlag)
    {
        //监测异常产生
        for(int i=0;i<isLaseLoseListOut.size();i++)
        {
            if(true==isLaseLoseListOut[i])
            {
//                if(E_VEHICLE_POSITION_TYPE_ROS==positonType)
//                {
//                    //amcl位置模式
//                    virtualLineControl->setLaserLose(true,0);
//                }
//                else
//                {
//                    //vmark位置模式
//                    virtualLineControl->setLaserLose(true,1);
//                }
                laserLoseRescueFlag=1;
//                qDebug()<<"监测到激光终止，，，laserLoseRescueFlag=1";
//                addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 5051, robotId);
                return -1;


            }
        }
    }
    else
    {
        //监测异常终止
        bool isAllLaserOk=true;
        for(int i=0;i<isLaseLoseListOut.size();i++)
        {
            if(true==isLaseLoseListOut[i])
            {
                isAllLaserOk=false;
            }
        }
        if(true==isAllLaserOk && true==tfOk)
        {
              laserLoseRescueFlag=0;
//            virtualLineControl->setLaserLose(false,0);//不安全，会导致暂停无效。
//            qDebug()<<"监测到激光恢复，，，laserLoseRescueFlag=0";
//            addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 5052, robotId);
            return 1;
        }
    }
    #endif
    return 1;

}

int TrajectoryServer::readVehicleType(QString filePath, E_VEHICLE_TYPE &typeOut)
{
    QVariant strConfigValue;
    QString firstTimeFlagStr = filePath;
    QString strConfigGroup = "";
    QString strConfigKey = "";

    QSettings settings(firstTimeFlagStr,QSettings::IniFormat);
    settings.setIniCodec("UTF-8");
    strConfigGroup = "MechanicalInfo";
    settings.beginGroup(strConfigGroup);

    //vehicletype
    strConfigKey = "vehicletype";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    typeOut= (E_VEHICLE_TYPE)strConfigValue.toInt();



    settings.endGroup();

    qDebug() << "readVehicleType sucess";
    return 1;
}

int TrajectoryServer::rosCommunicate()
{

    #ifdef D_USE_ROS
    //set odom
    VehicleVelocity tmpVehicleVelFeedback;
    VehiclePosition tmpVehiclePositionFeedback;
    getOdometer(tmpVehicleVelFeedback,tmpVehiclePositionFeedback);
    int returnFlag;
    OdemeterData odemeterData;
    odemeterData.pos=tmpVehiclePositionFeedback;
    odemeterData.vel=tmpVehicleVelFeedback;

    //m_rosApi->setWheelOdometerData(userInfo,robotId,odemeterData,returnFlag);
    m_rosApi->setWheelOdometerData_noRos(userInfo,robotId,odemeterData,returnFlag);

    PositionInfo tmpPositionInfo;
    tmpPositionInfo.isUpdateSucess=true;
    tmpPositionInfo.position=tmpVehiclePositionFeedback;
    clock_gettime(CLOCK_MONOTONIC, &tmpPositionInfo.matchSucessTimeStmap);
    vehiclePositionManage->updateVehiclePosition(tmpPositionInfo,E_VEHICLE_POSITION_TYPE_ENCODER_ODOM);
    vehiclePositionManage->updateVehicleVelocity(tmpVehicleVelFeedback,tmpPositionInfo.matchSucessTimeStmap);
    m_rosApi->updateVehicleVelocity(tmpVehicleVelFeedback,tmpPositionInfo.matchSucessTimeStmap);
    //设置速度，计算里程表
     vehiclePositionManage->updateVehicleVelocity(tmpVehicleVelFeedback,tmpPositionInfo.matchSucessTimeStmap);


    //getVehiclePosition
    PositionInfo feedback_ros;
    PositionInfo feedback_visionMatch;
//    m_rosApi->updateVehiclePositionOnce();
//    m_rosApi->getVehiclePositionFrom_tf();//保证不被amcl节点卡住，放在这里。
//    m_rosApi->getVehiclePosition_ros(userInfo,feedback_ros);
    if(1==m_rosApi->getVehiclePositionFrom_ros(feedback_ros))
    {
            vehiclePositionManage->updateVehiclePosition(feedback_ros,E_VEHICLE_POSITION_TYPE_ROS);
    }

    //pclicp
//    m_rosApi->updatePciIcpPosition();
    m_rosApi->getVehiclePosition_pclicp(userInfo,feedback_visionMatch);
    vehiclePositionManage->updateVehiclePosition(feedback_visionMatch,E_VEHICLE_POSITION_TYPE_PCL_MATCH);

    //vmark
//    m_rosApi->updateVmarkPosition();//一堵墙时，很消耗时间，把该运算迁移到ｒｏｓ定时器
    m_rosApi->getVehiclePositionInfo_vmark(userInfo,feedback_visionMatch);
    vehiclePositionManage->updateVehiclePosition(feedback_visionMatch,E_VEHICLE_POSITION_TYPE_VMARK);
    VehiclePosition uncertainPoseOut;
    m_rosApi->getVmarkPositionCertainity(uncertainPoseOut);
    vehiclePositionManage->setReflectPositionCertainity(uncertainPoseOut,E_VEHICLE_POSITION_TYPE_VMARK);



    //cu1Position
    if(true==cu1Sensor->isUseThisModule())
    {
        int tmpType=cu1Sensor->getVehiclePosition(feedback_visionMatch);
        if(0==tmpType)
        {
            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5047,robotId);
        }
        else if(-1==tmpType)
        {
            addMsg(ENUM_MSG_WARNING,"MotionServer","trajectory",5049,robotId);//不能错误，否则无法手动
        }
        else if(-2==tmpType)
        {
//            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5048,robotId);
        }
        else if(1==tmpType)
        {
//            vehiclePositionManage->updateVehiclePosition(feedback_visionMatch,E_VEHICLE_POSITION_TYPE_CU1);
        }
        vehiclePositionManage->updateVehiclePosition(feedback_visionMatch,E_VEHICLE_POSITION_TYPE_CU1);
    }
    else
    {
        //reflect
        VehiclePosition uncertainPoseOut;
        m_rosApi->getReflectPositionCertainity(uncertainPoseOut);
        vehiclePositionManage->setReflectPositionCertainity(uncertainPoseOut,E_VEHICLE_POSITION_TYPE_CU1);


        m_rosApi->getVehiclePositionInfo_reflect(userInfo,feedback_visionMatch);
        vehiclePositionManage->updateVehiclePosition(feedback_visionMatch,E_VEHICLE_POSITION_TYPE_CU1);

    }

    //qrcodePosition
    if(true==qrcodeSensorAdapter->isUseThisModule())
    {
        int tmpType=qrcodeSensorAdapter->getVehiclePosition(feedback_visionMatch);
        if(0==tmpType)
        {
            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5050,robotId);
        }
        else if(-2==tmpType)
        {
//            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5048,robotId);
        }
        else if(1==tmpType)
        {
//            vehiclePositionManage->updateVehiclePosition(feedback_visionMatch,E_VEHICLE_POSITION_TYPE_CU1);
        }
        vehiclePositionManage->updateVehiclePosition(feedback_visionMatch,E_VEHICLE_POSITION_TYPE_QRCODE);
    }

    #endif

    return 1;

}

int TrajectoryServer::vehicleEncoderOdomUpdate()
{
    //read odom feed back
    QVector<double> position,wheelVelFeedback,acc,torque;
    getFeedbackJoint(position,wheelVelFeedback,acc,torque);//行走轮在前，转向轴在后
    QVector<double> steeringPosition;
    getAgvSteeringPositionFeedback(steeringPosition);
    int returnType=vehicleJogControl->odometerCalculationLoop(wheelVelFeedback,position,steeringPosition);
    if(-2==returnType && jointsNum >=2)
    {
       addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5032,robotId);
       return -1;
    }
    return 1;
}

int TrajectoryServer::vehicleJogUpdate()
{
//    qDebug()<<"vehicleJogUpdate()";//yuguang
    //calculate and send vehicle motor vel command
    QVector<double> wheelVel;
    QVector<double> steeringAngle;
    //逆解失败时，返回的速度为０，转角不变。
    int returnKey=vehicleJogControl->moveLoop(wheelVel,steeringAngle);

    pthread_mutex_lock( &trajMutex);//写运动指令时加锁
    axisGroup->setAgvAxisVel(wheelVel);
    axisGroup->setAgvSteeringPosition(steeringAngle);
    pthread_mutex_unlock( &trajMutex);//写完后解锁

    return 1;
}

int TrajectoryServer::zeroManagementLoopRealTime_ethercat()
{
    if(1!=goZeroEnableFlag)
    {
//        qDebug()<<"1!=goZeroEnableFlag"<<goZeroEnableFlag;
        return 0;
    }
    int tmpAxisId;
    if(1==axisGroup->getGoZeroAxis(tmpAxisId))
    {
        if(homeFinishedCommand.axisId!=tmpAxisId)
        {
            startGoZero(tmpAxisId);//轴变化了,确保下个周期才判断状态。
            homeFinishedCommand.waitCount=0;
            homeFinishedCommand.axisId=tmpAxisId;
        }
        else
        {
            homeFinishedCommand.waitCount++;
            if(homeFinishedCommand.waitCount>100)
            {
                EM_ZERO_MOVING_STATUS statusOut;
                getZeroMovingStatus(tmpAxisId,statusOut);
//                qDebug()<<"_________________statusOut"<<statusOut<<"waitCount"<<homeFinishedCommand.waitCount<<"tmpAxisId"<<tmpAxisId;
                if(EM_ZERO_MOVING_STATUS_ERROR==statusOut)
                {
                    qDebug()<<"error,ethercat return zero,,,TrajectoryServer::zeroManagementLoopRealTime";
                    addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5038,robotId,tmpAxisId);
                }
                else if(EM_ZERO_MOVING_STATUS_MOVING==statusOut)
                {

                }
                else if(EM_ZERO_MOVING_STATUS_FINISHED==statusOut)
                {
                    qDebug()<<"zeroReturn finished,,,tmpAxisId="<<tmpAxisId;
                    addMsg(ENUM_MSG_REMIND,"MotionServer","trajectory",5039,robotId,tmpAxisId);

                    EM_ZERO_MOVINT_TYPE zeroMovingType;
                    axisGroup->getZeroMovingType(zeroMovingType);
                    if(EM_ZERO_MOVINT_TYPE_ALL_AXISES==zeroMovingType)
                    {
                        if(1!=axisGroup->changeToNextGoZeroAxis())
                        {
                            homeFinishedCommand.isFinished=true;

                        }
                    }
                    else if(EM_ZERO_MOVINT_TYPE_ONE_AXIS==zeroMovingType)
                    {
                        homeFinishedCommand.isFinished=true;


                    }
                }

            }


        }


    }
    else
    {
        if(false==homeFinishedCommand.isFinished)
        {
            homeFinishedCommand.isExit=true;
            qDebug()<<"error, no axis to go zero.ethercat mode";
            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5043,robotId);
        }
    }


    return 1;
}

int TrajectoryServer::zeroManagementLoopRealTime()
{
    if(E_ZERO_TASK_MODE_ETHERCAT==axisGroup->getZeroTaskMode())
    {
        return zeroManagementLoopRealTime_ethercat();
    }
    else
    {
        return zeroManagementLoopRealTime_inner();
    }
    return 1;
}

int TrajectoryServer::zeroManagementLoopRealTime_inner()
{

    int tmpAxisId;

    EM_ZERO_MOVING_STATUS statusOut=EM_ZERO_MOVING_STATUS_MOVING;
    if(1==axisGroup->getGoZeroAxis(tmpAxisId))
    {
//        startGoZero(tmpAxisId);
        QVector<ZeroReturnParameter> parameterOut;
        axisGroup->getZeroReturnParameter(parameterOut);
        bool tmpSensorValue;
        robCtl->getRobotDi(robotId,parameterOut[tmpAxisId].zeroSensorIndex,tmpSensorValue);
        switch(parameterOut[tmpAxisId].goZeroMethod)
        {
        case 1://负极限１个常开开关
        {
            tmpSensorValue=!tmpSensorValue;//!逻辑取反，～按位取反
        }
        case 0://负极限１个常闭开关。闭合：负方向快速运动，开关断开停止运动，反向低速运动到开关闭合。断开：正方向低速运动，开关断开停止运动
        {
            switch(zeroMovingStage)
            {
            case 0:
            {
                if(tmpSensorValue)
                {
                    zeroMovingStage=1;
                }
                else
                {
                    zeroMovingStage=2;
                }
                break;
            }
            case 1:
            {
                if(tmpSensorValue)
                {
                    axisGroup->moveOneStepByPosition_vel(-parameterOut[tmpAxisId].goZeroSpeed,tmpAxisId);
                }
                else
                {
                    zeroMovingStage=2;
                }
                break;
            }
            case 2:
            {
                if(tmpSensorValue)
                {
                    zeroMovingStage=3;
                }
                else
                {
                    axisGroup->moveOneStepByPosition_vel(parameterOut[tmpAxisId].goZeroSlowSpeed,tmpAxisId);

                }
                break;
            }
            case 3:
            {
                statusOut=EM_ZERO_MOVING_STATUS_FINISHED;
                break;
            }
            }

            break;
        }

        }


        if(EM_ZERO_MOVING_STATUS_ERROR==statusOut)
        {
            zeroMovingStage=0;
            qDebug()<<"error,inner return zero,,,TrajectoryServer::zeroManagementLoopRealTime";
            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5038,robotId,tmpAxisId);
        }
        else if(EM_ZERO_MOVING_STATUS_MOVING==statusOut)
        {
//             zeroMovingStage++;
        }
        else if(EM_ZERO_MOVING_STATUS_FINISHED==statusOut)
        {
            zeroMovingStage=0;
            qDebug()<<"zeroReturn finished,,,tmpAxisId="<<tmpAxisId;
            addMsg(ENUM_MSG_REMIND,"MotionServer","trajectory",5039,robotId,tmpAxisId);

            EM_ZERO_MOVINT_TYPE zeroMovingType;
            axisGroup->getZeroMovingType(zeroMovingType);
            if(EM_ZERO_MOVINT_TYPE_ALL_AXISES==zeroMovingType)
            {
                if(1!=axisGroup->changeToNextGoZeroAxis())
                {
                    homeFinishedCommand.isFinished=true;

                }
            }
            else if(EM_ZERO_MOVINT_TYPE_ONE_AXIS==zeroMovingType)
            {
                homeFinishedCommand.isFinished=true;


            }
        }
    }
    else
    {
        if(false==homeFinishedCommand.isFinished)
        {
            zeroMovingStage=0;
            homeFinishedCommand.isExit=true;
            qDebug()<<"error, no axis to go zero,inner logic go zero";
            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5043,robotId);
        }
    }


    return 1;
}

int TrajectoryServer::zeroManagementLoopCommonTime()
{
    //更新零点数据过程中，不运行下发电机位置给驱动器。必须使用反馈同步命令值后，才可以下发。
    if(true==homeFinishedCommand.isFinished)
    {
        if(true==homeFinishedCommand.isOneAxis)
        {
            isMotorCommandValid=false;
            axisGroup->calibrateRobot(homeFinishedCommand.axisId);
            setMotionServerModel(ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS);
            restorNormalOperationMode(homeFinishedCommand.axisId);
            qDebug()<<"zeroReturn  finished,,,EM_ZERO_MOVINT_TYPE_ONE_AXIS servoOff_all";
            addMsg(ENUM_MSG_REMIND,"MotionServer","trajectory",5039,robotId,homeFinishedCommand.axisId);
            servoOff_all();
            pthread_mutex_lock(&trajMutex);//test
            calibrateReset();
            pthread_mutex_unlock(&trajMutex);//test
            synchronizeMotorFeedbackToCalculatePosition();
            sleep(5);
            isMotorCommandValid=true;
        }
        else if(false==homeFinishedCommand.isOneAxis)
        {
            isMotorCommandValid=false;
            axisGroup->calibrateRobot();
            setMotionServerModel(ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS);
            restorNormalOperationMode();
            qDebug()<<"zeroReturn  finished,,,EM_ZERO_MOVINT_TYPE_ALL_AXISES servoOff_all";
            addMsg(ENUM_MSG_REMIND,"MotionServer","trajectory",5040,robotId);
            servoOff_all();

            pthread_mutex_lock(&trajMutex);//test
            calibrateReset();
            pthread_mutex_unlock(&trajMutex);//test
            synchronizeMotorFeedbackToCalculatePosition();

            sleep(5);
            isMotorCommandValid=true;
        }
    }
    else if(true==homeFinishedCommand.isExit)
    {
        setMotionServerModel(ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS);
        restorNormalOperationMode();
    }
    return 1;
}

int TrajectoryServer::vehicleJogLoopCommonTime()
{

    return 1;
}



int TrajectoryServer::setTimeAxisScaleByTrap(double ratio,int timeAxisIndex, int acceleration)
{
    pthread_mutex_lock(&trajMutex);//test
    axisGroup->setTimeAxisScaleByTrap(ratio,timeAxisIndex, acceleration);
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}

int TrajectoryServer::setTimeAxisScaleBySpline(double ratio,int timeAxisIndex)
{
    pthread_mutex_lock(&trajMutex);//test
    axisGroup->setTimeAxisScaleBySpline(ratio,timeAxisIndex);
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}


int TrajectoryServer::getCurrentToolForwardMatrix(Frame &frameReturn)
{
    if(basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED)
    {
        qDebug()<<"getCurrentToolForwardMatrix error, basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED.";
        return -1;
    }
    pthread_mutex_lock(&trajMutex);//test
    int tmpReturn= coordinateManager->getCurrentToolForwardMatrix(frameReturn);
    pthread_mutex_lock(&trajMutex);//test
    return tmpReturn;

}

int TrajectoryServer::getCurrentToolInverseMatrix(Frame &frameReturn)
{
    if(basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED)
    {
        qDebug()<<"getCurrentToolForwardMatrix error, basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED.";
        return -1;
    }
    pthread_mutex_lock(&trajMutex);//test
    int tmpReturn=  coordinateManager->getCurrentToolInverseMatrix(frameReturn);
    pthread_mutex_lock(&trajMutex);//test
    return tmpReturn;
}

int TrajectoryServer::getCurrentUserForwardMatrix(Frame &frameReturn)
{
    if(basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED)
    {
        qDebug()<<"getCurrentToolForwardMatrix error, basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED.";
        return -1;
    }
    pthread_mutex_lock(&trajMutex);//test
    int tmpReturn=  coordinateManager->getCurrentUserForwardMatrix(frameReturn);
    pthread_mutex_unlock(&trajMutex);//test
    return tmpReturn;
}

int TrajectoryServer::getCurrentUserInverseMatrix(Frame &frameReturn)
{
    if(basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED)
    {
        qDebug()<<"getCurrentToolForwardMatrix error, basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED.";
        return -1;
    }
    pthread_mutex_lock(&trajMutex);//test
    int tmpReturn=  coordinateManager->getCurrentUserInverseMatrix(frameReturn);
    pthread_mutex_unlock(&trajMutex);//test
    return tmpReturn;
}


int TrajectoryServer::addStopTraJectory()
{
    if(ENUM_TRAJECTORY_TOSTOP!=basicStatus)
    {
        return -1;
    }
    clearTraj(false);

    MoveParameter tmpMovePara;
    PointPro targetPoint;
    targetPoint.jointFlag=JOINT_FLAG_JOINT;
    targetPoint.pointNum=9;
    targetPoint.groupNum=1;
    targetPoint.ufValue=0;
    targetPoint.utValue=0;
    targetPoint.recordStatus=1;
    targetPoint.configString="r, t,s ,1,1,1";
    targetPoint.configInt.resize(7);
    targetPoint.positionValue.resize(innerJointsNum);
    QVector<double> tmpJointsPosition, tmpJointsVel, tmpJointsAcc, tmpJointsTorque;
    axisGroup->getJointFeedback(tmpJointsPosition, tmpJointsVel, tmpJointsAcc, tmpJointsTorque);

    //hualei todo jointdecouple 速度　力矩　是否正确，单位换算，r/min ,百分比　力矩
    for(int i=0;i<innerJointsNum;i++)
    {
        targetPoint.positionValue[i]=tmpJointsPosition[i]+tmpJointsVel[i]/2*0.5;//0.5秒钟速度减为０的距离停止
    }
//    for(int i=0;i<externJointsNum;i++)
//    {
//        targetPoint.extPositionValue[i]=tmpJointsPosition[i+innerJointsNum]+tmpJointsVel[i+innerJointsNum]/2*0.5;//0.5秒钟速度减为０的距离停止
//    }

//    //仅仿真测试用 hualei todo 外部轴stop没有处理
//    axisGroup->getJointCommand(tmpJointsPosition, tmpJointsVel, tmpJointsAcc, tmpJointsTorque);
//    for(int i=0;i<innerJointsNum;i++)
//    {
//        targetPoint.positionValue[i]=tmpJointsPosition[i]+tmpJointsVel[i]/2*0.3;//0.3秒钟速度减为０的距离停止
//    }
    //真实机器获取当前位置　速度
//    targetPoint.extPositionValue.resize(externJointsNum);
//    for(int i=0;i<externJointsNum;i++)
//    {
//        targetPoint.extPositionValue[i]=tmpJointsPosition[innerJointsNum+i]+tmpJointsVel[innerJointsNum+i]/2*1;//1秒钟速度减为０的距离停止
//    }

    tmpMovePara.moveType = ENUM_COMMAND_JOINT_CONTINIOUS;
    tmpMovePara.velocityUnit = ENUM_TIME_DURATION;
    tmpMovePara.velocity = 1;//s
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 3;
    tmpMovePara.cntRatio = 20;
    tmpMovePara.acc = 65;
    tmpMovePara.lineNumber = ENUM_LINE_STOP;
    tmpMovePara.targetPoint = targetPoint;


    pthread_mutex_lock( &trajMutex);//写运动指令时加锁
    commandQueue.enqueue(tmpMovePara);
    pthread_mutex_unlock( &trajMutex);//写完后解锁

    setBasicStatus(ENUM_TRAJECTORY_STOPPING,9990010);
    axisGroup->setCheckMode(E_CHECK_EMERGENCY_STOP);

    setTimeAxisScale(1.01,0.05);

    return 1;

}


 int TrajectoryServer::addTrajectoryCommand(QVector<MoveParameter> tmpCommandVector)
 {
     if( basicStatus==ENUM_TRAJECTORY_STOPPING)
     {
         return -1;
     }
     if(ENUM_MOTION_MODEL_DRAG==motionModel)
     {
         addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 5080, robotId);
         return -2;
     }

     printInformation(tmpCommandVector,"TrajectoryServer::addTrajectoryCommand");

     if(isInCarvingTp)
     {
         setIsInCarvingMode(false);
     }

     pthread_mutex_lock( &trajMutex);//写运动指令时加锁
     for(int i = 0; i < tmpCommandVector.size();i++)
     {
         commandQueue.enqueue(tmpCommandVector[i]);
     }
     pthread_mutex_unlock( &trajMutex);//写完后解锁

     return 1;
 }


 int TrajectoryServer::getCommandQueueSize()
 {
     int tmpSize;
     pthread_mutex_lock( &trajMutex);//出队列前加锁
     tmpSize=commandQueue.size();
     pthread_mutex_unlock( &trajMutex);
     return tmpSize;
 }


int TrajectoryServer::getCommandFromQueue(MoveParameter & moveParameter)
{
    pthread_mutex_lock( &trajMutex);//出队列前加锁
    if(commandQueue.isEmpty())
    {
        pthread_mutex_unlock( &trajMutex);
        return 0;
    }
    moveParameter = commandQueue.dequeue();
    if( (1==moveParameter.targetPoint.jointFlag)&&//笛卡尔点
            ((moveParameter.targetPoint.ufValue != coordinateManager->getCurrentUserIndex()) ||//用户坐标系编号不一致
             (moveParameter.targetPoint.utValue != coordinateManager->getCurrentToolIndex())) )//工具坐标系编号不一致
    {
        pthread_mutex_unlock( &trajMutex);
        if(debugFlag)
        {
            qDebug()<< "McControllerInterpreter::addTrajectoryCommandFromCommandQueue====笛卡尔点坐标系编号不一致"\
                    << moveParameter.targetPoint.ufValue<<", "<<moveParameter.targetPoint.utValue;
        }

        addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5019, robotId, moveParameter.lineNumber);
        return -1;
    }
    pthread_mutex_unlock( &trajMutex);


    return 1;
}



 int TrajectoryServer::calculateTrajectoryPath_cnt()
 {

     motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH;
     MoveParameter moveParameter,moveParameter2;
     if(0==getCommandFromQueue(moveParameter))
     {
         //qDebug()<<"TrajectoryServer::calculateTrajectoryPath(), return 0";
         motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_NOCOMMAND;
         motionDebugInfo.commandQueueSize=0;
         return 0;
     }
     motionDebugInfo.commandQueueSize=getCommandQueueSize();

     int processResult=-1;

     //以前设计是支持队列缓存，现在修改为不支持缓存(有问题，有时候会不为空）.如果生成轨迹失败，则一直循环尝试。
     //如果命令队列中有太多运动命令，会导致一直死循环，从而cpu占用过高，然后customtime过多，ethercat时序出问题卡顿．
     motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_WHILE;
     if(false==isCoordinatePlanInitialed)
     {
         pthread_mutex_lock(&trajMutex);//test
         resetCoordinateTrajPlanCondition();
         pthread_mutex_unlock(&trajMutex);//test

     }
     if(ENUM_COMMAND_CIRCLE==moveParameter.moveType)
     {
         if(0==getCommandFromQueue(moveParameter2))
         {
             return -2;
         }
     }
     if(0==moveParameter.cntRatio)
     {//单条
         processResult=calculateOnePath(moveParameter,moveParameter2);
     }
     else
     {//多条平滑
             bool isLoop;
             do
             {
                processResult=calculateOnePath(moveParameter,moveParameter2);
                if(-1==processResult)
                {
                    addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5021,robotId);
                    return -1;
                }
                isLoop=true;

                if(1==getCommandFromQueue(moveParameter))
                {
                    if(ENUM_COMMAND_CIRCLE==moveParameter.moveType)
                    {
                        if(0==getCommandFromQueue(moveParameter2))
                        {
                            qDebug()<<"------error,,,0==getCommandFromQueue(moveParameter2)";
                            return -2;
                        }
                        if(0==moveParameter2.cntRatio)
                        {
                            isLoop=false;
                        }
                    }
                    else
                    {
                        if(0==moveParameter.cntRatio)
                        {
                            isLoop=false;
                        }
                    }
                }
                else
                {
                    qDebug()<<"------error,,,0==getCommandFromQueue(moveParameter) calculateTrajectoryPath_cnt";
                    if(basicStatus==ENUM_TRAJECTORY_STOPPING||basicStatus==ENUM_TRAJECTORY_STOPPED)
                    {
                        return -200;
                    }
                }

             }
             while(isLoop);
             if(0==moveParameter.cntRatio)
             {
                processResult=calculateOnePath(moveParameter,moveParameter2);
             }

     }

     //updateTrajTime(), 偶尔出现，行号确实完成了,当是trajProfile还没有来得及清除，导致添加失败。
     if(-1==processResult)
     {
         addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5021,robotId);
         return -1;
     }
 //         停止的时候跳出
//         usleep(1000);
//         if(basicStatus==ENUM_TRAJECTORY_STOPPING||basicStatus==ENUM_TRAJECTORY_STOPPED)
//         {
//             motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_WHILE_BREAK;
//             return -1;
//         }

     motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_NORMAL_FINISH;
     return processResult;
 }

 int TrajectoryServer::processQueueForCarvingModule()
 {
     motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH;
     MoveParameter moveParameter;
     while(1==getCommandFromQueue(moveParameter))
     {
         printInformation(moveParameter,"processLineInstruction-CarvingModule");
         if(ENUM_COMMAND_LINE!=moveParameter.moveType)
         {
             qDebug()<<"连续插补错误1 ENUM_COMMAND_LINE!=moveParameter.moveType";
             addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5085,robotId);
             return -1;
         }
         if(JOINT_FLAG_CARTESIAN!=moveParameter.targetPoint.jointFlag)
         {
             qDebug()<<"连续插补错误2 JOINT_FLAG_CARTESIAN!=moveParameter.targetPoint.jointFlag";
             addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5084,robotId);
             return -1;
         }


         if(1!=carvingModule->addLine(moveParameter.lineNumber,moveParameter.targetPoint.positionValue,
                                 roboParameter->lineMaxSpeed*moveParameter.velocity,roboParameter->lineMaxAcceleration))
         {
             qDebug()<<"连续插补错误3";
             addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5083,robotId);
             return -1;
         }

     }
     motionDebugInfo.commandQueueSize=getCommandQueueSize();

     motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_NORMAL_FINISH;
     return 1;
 }

 int TrajectoryServer::resetCoordinateTrajPlanCondition()
 {

     axisGroup->setJointPlanStartPosition_byPositionCommand();
     setPositionFrameCommand_byJointCommand();

     isCoordinatePlanInitialed=true;//trackEnd时会设置false
     return 1;
 }

 int TrajectoryServer::transCommandQueueToCarvingModule(QVector<MoveParameter> &tmpCommandVectorIn)
 {

     if(false==isInCarvingTp)
     {
         setIsInCarvingMode(true);
     }

     for(int index=0;index<tmpCommandVectorIn.size();index++)
     {
         printInformation(tmpCommandVectorIn[index],"processLineInstruction-CarvingModule");
         if(1!=pointProcessForTraj(tmpCommandVectorIn[index].lineNumber,tmpCommandVectorIn[index].targetPoint))
         {
             return -1;
         }


         int tmpMoveType=tmpCommandVectorIn[index].moveType;
         if(ENUM_COMMAND_LINE!=tmpMoveType && ENUM_COMMAND_CIRCLE!=tmpMoveType)
         {
             qDebug()<<"连续插补错误1 ENUM_COMMAND_LINE!=moveParameter.moveType"<<tmpMoveType;
             addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5085,robotId);
             return -1;
         }
//         if(JOINT_FLAG_CARTESIAN!=tmpCommandVectorIn[index].targetPoint.jointFlag)
//         {
//             qDebug()<<"连续插补错误2 JOINT_FLAG_CARTESIAN!=moveParameter.targetPoint.jointFlag";
//             addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5084,robotId);
//             return -1;
//         }

         if(ENUM_COMMAND_LINE==tmpMoveType)
         {
             double tmpVelIn,tmpAcc;
             if(ENUM_RATIO==tmpCommandVectorIn[index].velocityUnit)
             {
                 tmpVelIn=roboParameter->lineMaxSpeed*tmpCommandVectorIn[index].velocity;
             }
             else
             {
                 tmpVelIn=tmpCommandVectorIn[index].velocity*100.0;
             }
             tmpAcc=roboParameter->lineMaxAcceleration;
             if(tmpCommandVectorIn[index].acc>0 && tmpCommandVectorIn[index].acc<2)
             {
                 tmpAcc=roboParameter->lineMaxAcceleration*tmpCommandVectorIn[index].acc;
             }
             transferRPY(tmpCommandVectorIn[index].targetPoint.positionValue,tmpCommandVectorIn[index].targetPoint.positionValue);
             if(1!=carvingModule->addLine(tmpCommandVectorIn[index].lineNumber,tmpCommandVectorIn[index].targetPoint.positionValue,
                                            tmpVelIn,tmpAcc))
             {
                 qDebug()<<"连续插补错误3";
                 addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5083,robotId);
                 return -1;
             }
         }
         else if(ENUM_COMMAND_CIRCLE==tmpMoveType)
         {
             //todo
//             qDebug()<<"连续插补错误,暂时不支持圆弧"<<tmpMoveType;
//             addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5085,robotId);
//             return -1;
             if(index+1==tmpCommandVectorIn.size())
             {
                 qDebug()<<"error,,,,圆弧规划缺少终止点";
                 addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5087,robotId);
                 return -1;
             }

             if(ENUM_COMMAND_CIRCLE!=tmpCommandVectorIn[index+1].moveType)
             {
                 qDebug()<<"error,,,,圆弧规划第二段非圆弧类型"<<tmpCommandVectorIn[index+1].moveType;
                 addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5088,robotId);
                 return -1;
             }

             if(1!=pointProcessForTraj(tmpCommandVectorIn[index+1].lineNumber,tmpCommandVectorIn[index+1].targetPoint))
             {
                 return -1;
             }


             //计算圆心和向量
             std::vector<double> centerOut, normalOut,tmpStartPos;
             tmpStartPos.resize(9);
             for(int i=0;i<tmpStartPos.size();i++)
             {
                 tmpStartPos[i]=tpBeforePose[i];
             }
             int transResult=transCircleInfo(tmpStartPos,tmpCommandVectorIn[index].targetPoint.positionValue
                             ,tmpCommandVectorIn[index+1].targetPoint.positionValue,centerOut, normalOut);
             if(1!=transResult)
             {
                 qDebug()<<"error,,,,transCircleInfo";
                 addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5090,robotId);
                 return -1;
             }
             double tmpVelIn,tmpAcc;
             if(ENUM_RATIO==tmpCommandVectorIn[index+1].velocityUnit)
             {
                 tmpVelIn=roboParameter->lineMaxSpeed*tmpCommandVectorIn[index+1].velocity;
             }
             else
             {
                 tmpVelIn=tmpCommandVectorIn[index+1].velocity*100.0;
             }
             tmpAcc=roboParameter->lineMaxAcceleration;
             if(tmpCommandVectorIn[index+1].acc>0 && tmpCommandVectorIn[index].acc<2)
             {
                 tmpAcc=roboParameter->lineMaxAcceleration*tmpCommandVectorIn[index+1].acc;
             }
             transferRPY(tmpCommandVectorIn[index+1].targetPoint.positionValue,
                                tmpCommandVectorIn[index+1].targetPoint.positionValue);
             if(1!=carvingModule->addCircle(tmpCommandVectorIn[index+1].lineNumber,
                                            tmpCommandVectorIn[index+1].targetPoint.positionValue,
                                            centerOut ,normalOut ,tmpVelIn,tmpAcc,0))
             {
                 qDebug()<<"连续插补错误 circle";
                 addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5089,robotId);
                 return -1;
             }
             index++;
         }

     }

     return 1;
 }

 int TrajectoryServer::calculateOnePath(MoveParameter moveParameter,MoveParameter moveParameter2)
 {
     //plan开始点会自动衔接上。setPositionFrameCommandPlaned_byJointCommandPlaned
     int processResult;
     switch(moveParameter.moveType)
     {
         case ENUM_COMMAND_JOINT_VARIABLE:
         {
             processResult=processJointInstruction(moveParameter,velocityStype_trajServer);

             break;
         }
         case ENUM_COMMAND_JOINT_SPLINE:
         {

             processResult=processJointInstruction(moveParameter,E_VELOCITY_TYPE_SPLINE);

             break;
         }
         case ENUM_COMMAND_SINGLE_AXIS_SPLINE:
         {
             processResult=processSingleAxisInstruction(moveParameter,E_VELOCITY_TYPE_SPLINE);

             break;
         }
         case ENUM_COMMAND_SINGLE_AXIS_TRAP:
         {
             processResult=processSingleAxisInstruction(moveParameter,velocityStype_trajServer);//velocityStype_trajServer E_VELOCITY_TYPE_TAPE

             break;
         }
         case ENUM_COMMAND_JOINT_TRAP:
         {
             processResult=processJointInstruction(moveParameter,E_VELOCITY_TYPE_TAPE);

             break;
         }

         case ENUM_COMMAND_LINE:
         {
             processResult=processLineInstruction(moveParameter);

             break;
         }
         case ENUM_COMMAND_CIRCLE:
         {
             processResult=processCircleInstruction(moveParameter,moveParameter2);
             break;
         }
         default:
         {
             qDebug()<<"Coordinate::addMsg====not match error code";
             return -1;
         }

     }

     return processResult;

 }

 int TrajectoryServer::jointInstructionCalculate(QVector<MoveParameter>& moveParameterArry,
                                                 MoveParameter moveParameter,int& queueGetedFlag)
 {
     int processResult;
     if(0==moveParameter.cntRatio)
     {//单条

         processResult=processJointInstruction(moveParameter,velocityStype_trajServer);//velocityStype_trajServer E_VELOCITY_TYPE_TAPE
         return processResult;

     }
     else
     {//多条平滑
         if(queueGetedFlag==0)
         {
             moveParameterArry.append(moveParameter);

             while(0!=getCommandFromQueue(moveParameter) && 0!=moveParameter.cntRatio)
             {
                moveParameterArry.append(moveParameter);
             }
             if(0==moveParameter.cntRatio)
             {
                moveParameterArry.append(moveParameter);
             }

             queueGetedFlag=1;
         }
         processResult=processJointContinousInstruction(moveParameterArry,0);

         return processResult;
     }
     return 0;
 }


 int TrajectoryServer::processLineInstruction(MoveParameter moveParameter)
 {
     printInformation(moveParameter,"processLineInstruction");
     if(1!=pointProcessForTraj(moveParameter.lineNumber,moveParameter.targetPoint))
     {
         return -1;
     }



     int tmpReturn=0;
     Frame targetFrame;
     if(1!=pointProToFrame(moveParameter.targetPoint,targetFrame))
     {
         addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5055,robotId);
         qDebug()<<"processLineInstruction error,axisGroup->lineMove ,1!=tmpReturn";
         return -1;
     }
     double timeDurationOut;
     pthread_mutex_lock(&trajMutex);//test
     //axisGroup->resetCoordinateTraj(0);
//     updatePositionFrameCommand();
     if(moveParameter.acc>0)
     {
         axisGroup->setAccelerationRatio_RobotArmLine(moveParameter.acc/100.0);
     }

     tmpReturn=axisGroup->lineMove(targetFrame,moveParameter.velocity,timeDurationOut,0,
                     axisGroup->getMixDurationLeft(),moveParameter.cntRatio,moveParameter.lineNumber);
     pthread_mutex_unlock(&trajMutex);//test
//     qDebug()<<"lineMove"<<targetFrame.p.data[0]<<targetFrame.p.data[1]<<targetFrame.p.data[2];
     if(1!=tmpReturn)
     {
//         addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5018,robotId);
         qDebug()<<"processLineInstruction error,axisGroup->lineMove ,1!=tmpReturn";
         return -1;
     }


     pthread_mutex_lock(&trajMutex);//test
     addTrajTime(moveParameter.lineNumber,timeDurationOut);
     pthread_mutex_unlock(&trajMutex);//test

     return 1;

 }

 int TrajectoryServer::processCartesianContinousInstruction(QVector<MoveParameter> moveParameterArry, int isLastPointMove)
 {
     printInformation( moveParameterArry, "processCartesianContinousInstruction");

     int pointCount=moveParameterArry.size();

     //点统一转化为迪卡尔点
     for(int i=0;i<pointCount;i++)
     {
        if(1!=pointCoordinateToCartesian(moveParameterArry[i].targetPoint,moveParameterArry[i].targetPoint))
        {
            qDebug()<<"----------error nddsfd";
            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5059,robotId);
            return -1;
        }
     }


     //点多解信息匹配检查
     vector<double> initialJointPosition;
     initialJointPosition.resize(innerJointsNum);
     QVector<double> jointsPositionOut;
     pthread_mutex_lock(&trajMutex);//test
     axisGroup->getJointCommand(jointsPositionOut);
     for(int i=0;i<innerJointsNum;i++)
     {
         initialJointPosition[i]=jointsPositionOut[i];
     }
     for(int i=0;i<pointCount;i++)
     {
         int futResutl=coordinateManager->isPointFutCorrect(moveParameterArry[i].targetPoint, initialJointPosition);

         if(1!=futResutl)
         {
             pthread_mutex_unlock(&trajMutex);//test
             addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5022,robotId);
             return -1;
         }
     }
     pthread_mutex_unlock(&trajMutex);//test



     int tmpReturn=0;
     QVector<Frame> targetFrame;
     targetFrame.resize(pointCount);
     QVector<double> speedPercent,timeDurationResult,cntList;
     QVector<int> accList;
     speedPercent.resize(pointCount);
     timeDurationResult.resize(pointCount);
     cntList.resize(pointCount);
     accList.resize(pointCount);
     for(int i=0;i<pointCount;i++)
     {
//        targetFrame[i]=pointProToFrame(moveParameterArry[i].targetPoint);
//        Frame targetFrame;
        if(1!=pointProToFrame(moveParameterArry[i].targetPoint,targetFrame[i]))
        {
            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5055,robotId);
            qDebug()<<"processCartesianContinousInstruction error ";
            return -1;
        }

        speedPercent[i]=moveParameterArry[i].velocity;
        cntList[i]=moveParameterArry[i].cntRatio;
        accList[i]=moveParameterArry[i].acc;
     }

     pthread_mutex_lock(&trajMutex);//test
//     updatePositionFrameCommand();
//     tmpReturn=axisGroup->lineContinuousMove(targetFrame,speedPercent,timeDurationResult,
//                                             moveParameterArry[0].cntRatio,0);

     tmpReturn=axisGroup->lineContinuousMove_mixedCNT(targetFrame,speedPercent,timeDurationResult,
                                             cntList,0,axisGroup->getMixDurationLeft(),accList);
     pthread_mutex_unlock(&trajMutex);//test
     if(1!=tmpReturn)
     {
//         addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5018,robotId);
         qDebug()<<"processLineInstruction error,axisGroup->lineMove ,1!=tmpReturn";
         return -1;
     }
     //外部轴判断处理
//     for(int i=0;i<pointCount;i++)
//     {
//         if(moveParameterArry[i].targetPoint.extPositionValue.size()==externJointsNum && externJointsNum>0)
//         {
//             QVector<int> axisIdVector;
//             axisIdVector.resize(externJointsNum);
//             QVector<double> targetPoint;
//             targetPoint.resize(externJointsNum);
//             for(int i =0;i<externJointsNum;i++)
//             {
//                 axisIdVector[i]=i+innerJointsNum;
//                 targetPoint[i]=moveParameterArry[i].targetPoint.extPositionValue[i];
//             }
//             pthread_mutex_lock(&trajMutex)1;
//             tmpReturn=axisGroup->syncSplineZeroMoveByTime(targetPoint,timeDurationResult[i],axisIdVector);
//             pthread_mutex_unlock(&trajMutex)1;
//             if(1!=tmpReturn)
//             {
//                 addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5018,robotId);
//                 qDebug()<<"processLineInstruction error,axisGroup->syncSplineZeroMoveByTime ,1!=tmpReturn";
//                 return -1;
//             }
//         }
//     }

     pthread_mutex_lock(&trajMutex);//test
     for(int i=0;i<pointCount;i++)
     {
        addTrajTime(moveParameterArry[i].lineNumber,timeDurationResult[i]);
     }
     pthread_mutex_unlock(&trajMutex);//test

     return 1;
 }

 int TrajectoryServer::processCircleInstruction(MoveParameter moveParameter1, MoveParameter moveParameter2)
 {
     printInformation(moveParameter1,"processCircleInstruction moveParameter1");
     printInformation(moveParameter2,"processCircleInstruction moveParameter2");
     if(1!=pointProcessForTraj(moveParameter1.lineNumber,moveParameter1.targetPoint))
     {
         return -1;
     }
     if(1!=pointProcessForTraj(moveParameter2.lineNumber,moveParameter2.targetPoint))
     {
         return -1;
     }



     int tmpReturn=0;
     QVector<Frame> targetFrameList;
     Frame targetFrame1;
     if(1!=pointProToFrame(moveParameter1.targetPoint,targetFrame1))
     {
         addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5055,robotId);
         qDebug()<<"processCircleInstruction error ";
         return -1;
     }

     Frame targetFrame2;
     if(1!=pointProToFrame(moveParameter2.targetPoint,targetFrame2))
     {
         addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5055,robotId);
         qDebug()<<"processCircleInstruction error ";
         return -1;
     }
     targetFrameList.append(targetFrame1);
     targetFrameList.append(targetFrame2);

     QVector<double> timeDurationOut;
     pthread_mutex_lock(&trajMutex);//test
     //axisGroup->resetCoordinateTraj(0);
//     updatePositionFrameCommand();
     if(moveParameter1.acc>0)
     {
         axisGroup->setAccelerationRatio_RobotArmLine(moveParameter1.acc/100.0);
     }
     tmpReturn=axisGroup->circleMove(targetFrameList,moveParameter2.velocity,timeDurationOut,0
                                     ,moveParameter2.cntRatio,moveParameter2.lineNumber);
     pthread_mutex_unlock(&trajMutex);//test
     if(1!=tmpReturn)
     {
//         addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5018,robotId);
         qDebug()<<"processLineInstruction error,axisGroup->lineMove ,1!=tmpReturn";
         return -1;
     }


     pthread_mutex_lock(&trajMutex);//test
     addTrajTime(moveParameter1.lineNumber,timeDurationOut[0]);
     addTrajTime(moveParameter2.lineNumber,timeDurationOut[1]+timeDurationOut[0]);
     pthread_mutex_unlock(&trajMutex);//test

     return 1;

 }

 int TrajectoryServer::processJointContinousInstruction(QVector<MoveParameter> moveParameterArry, int isLastPointMove)
 {
     printInformation( moveParameterArry, "processJointContinousInstruction");

     int pointCount=moveParameterArry.size();
     if(0==pointCount)
     {
          qDebug()<<" error: 0==pointCount!!!";
         return -1;
     }
     for(int i=0;i<pointCount;i++)
     {
         //点统一转化为关节点
//         moveParameterArry[i].targetPoint=pointCoordinateToJoint(moveParameterArry[i].targetPoint);
         PointPro targetPointIn=moveParameterArry[i].targetPoint;
         //pointCoordinateToJoint_noDeviationCheck
         if(1!=pointCoordinateToJoint_noDeviationCheck(targetPointIn,moveParameterArry[i].targetPoint))
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5053,robotId,jointsNum);
             qDebug()<<"TrajectoryServer:: processJointInstruction moveParameter error!!!";
             return -101;
         }
         if(moveParameterArry[i].targetPoint.positionValue.size()
                 +moveParameterArry[i].targetPoint.extPositionValue.size()<jointsNum)
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5035,robotId,jointsNum);
             qDebug()<<"TrajectoryServer:: processJointInstruction moveParameter error!!!positionValue size"
                    <<moveParameterArry[i].targetPoint.positionValue.size();
             return -100;
         }

     }

    QVector<int> axisIdVector;
    axisIdVector.resize(jointsNum);
    QVector<QVector<VelPoint> > velPointArry;
    velPointArry.resize(jointsNum);
    //外部轴数据处理
//    for(int k =0;k<pointCount;k++)
//    {
//        for(int i =0;i<externJointsNum;i++)
//        {
//            moveParameterArry[k].targetPoint.positionValue.push_back(moveParameterArry[k].targetPoint.extPositionValue[i]);
//        }
//    }

    for(int i =0;i<jointsNum;i++)
    {
        axisIdVector[i]=i;
        velPointArry[i].resize(pointCount);
        for(int k =0;k<pointCount;k++)
        {
            velPointArry[i][k].positionIn=moveParameterArry[k].targetPoint.positionValue[i];
            velPointArry[i][k].accIn=0;//使经过点速度为０，否这移动距离接近０的轴会动。
            if(moveParameterArry[k].velocityUnit!=ENUM_RATIO && ENUM_TIME_DURATION!=moveParameterArry[k].velocityUnit)
            {
                addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5012,robotId);
                return -101;
            }
            else if(moveParameterArry[k].velocityUnit==ENUM_RATIO)
            {
                velPointArry[i][k].velIn=moveParameterArry[k].velocity;//轴组根据平均值估算
                velPointArry[i][k].timeDuration=-1;//-1时，用各个轴的最大速度，正数时，表示路径时间
            }
            else if(moveParameterArry[k].velocityUnit==ENUM_TIME_DURATION)
            {
                velPointArry[i][k].velIn=-1;//轴组根据平均值估算
                velPointArry[i][k].timeDuration=moveParameterArry[k].velocity;//-1时，用各个轴的最大速度，正数时，表示路径时间
            }

        }
    }

    int tmpReturn;
    QVector<double> tmpTimeDurationListResult;
    pthread_mutex_lock(&trajMutex);//test
    TimeTool::getInstance()->recordInTime(timer_id3[E_TIME3_C8]);

    tmpReturn=axisGroup->splineContinueMoveMultiPoint(velPointArry,axisIdVector,isLastPointMove,tmpTimeDurationListResult);
    if(1==tmpReturn)
    {
        for(int i =0;i<pointCount;i++)
        {
            addTrajTime(moveParameterArry[i].lineNumber,tmpTimeDurationListResult[i]);
        }
    }
    TimeTool::getInstance()->recordOutTime(timer_id3[E_TIME3_C8]);
    pthread_mutex_unlock(&trajMutex);//test

    return tmpReturn;
 }


 int TrajectoryServer::processJointInstruction(MoveParameter moveParameter,E_VELOCITY_TYPE velocityType)
 {
     printInformation(moveParameter,"processJointInstruction");

     //点统一转化为关节点
     PointPro tmpPoint=moveParameter.targetPoint;
//     pointCoordinateToCartesian(moveParameter.targetPoint);
//     userFrameToPointPro_JOINT();/
     //Pr寄存器关节运动，不检查轴的变换偏差。普通点，则需要根据左右手求解。
     if(moveParameter.targetPoint.configString.empty())
     {
         //pointCoordinateToJoint_noDeviationCheck
         if(1!=pointCoordinateToJoint_noDeviationCheck(tmpPoint,moveParameter.targetPoint))
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5057,robotId,jointsNum);
             qDebug()<<"TrajectoryServer:: processJointInstruction moveParameter error!!!";
             return -101;
         }
     }
     else
     {
         if(1!=pointCoordinateToJoint(tmpPoint,moveParameter.targetPoint))
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5057,robotId,jointsNum);
             qDebug()<<"TrajectoryServer:: processJointInstruction moveParameter error!!!";
             return -101;
         }
     }

//     if(moveParameter.targetPoint.positionValue.size()+moveParameter.targetPoint.extPositionValue.size()<jointsNum)
//     {
//         addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5035,robotId,jointsNum);
//         qDebug()<<"TrajectoryServer:: processJointInstruction moveParameter error!!!positionValue size"
//                <<moveParameter.targetPoint.positionValue.size();
//         return -100;
//     }

    QVector<int> axisIdVector;
    QVector<double> targetPoint;
//    axisIdVector.resize(jointsNum);
//    targetPoint.resize(jointsNum);
//    for(int i =0;i<innerJointsNum;i++)
//    {
//        axisIdVector[i]=i;
//        targetPoint[i]=moveParameter.targetPoint.positionValue[i];
//    }

    //设定轴和角度
    for(int i =0;i<moveParameter.targetPoint.positionValue.size();i++)
    {
        //如果是耦合跟随轴或龙门跟随轴就不要控制
        if(true!=isGantryControlFollowAxisWork(i) && true!=isCoupleControlFollowAxisWork(i))
        {
            axisIdVector.append(i);
            targetPoint.append(moveParameter.targetPoint.positionValue[i]);
        }

    }
//    for(int i =0;i<externJointsNum;i++)
//    {
//        axisIdVector[i+innerJointsNum]=i+innerJointsNum;
//        targetPoint[i+innerJointsNum]=moveParameter.targetPoint.extPositionValue[i];
//    }

    if(moveParameter.velocityUnit!=ENUM_RATIO)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5012,robotId);
        return -101;
    }


     int tmpReturn=0;
     double tmpTime;
     pthread_mutex_lock(&trajMutex);//test
     TimeTool::getInstance()->recordInTime(timer_id3[E_TIME3_C6]);

     if(moveParameter.acc>0)
     {
         axisGroup->setAccelerationRatio_RobotArmJoint(moveParameter.acc/100.0);
     }

     tmpReturn=axisGroup->syncZeroMove_velVariable(velocityType,targetPoint,
                                      moveParameter.velocity, axisIdVector,tmpTime,
                  axisGroup->getMixDurationLeft(),moveParameter.cntRatio,moveParameter.lineNumber);

     if(1==tmpReturn)
     {
         addTrajTime(moveParameter.lineNumber,tmpTime);
     }
     else
     {
//         qDebug()<<"TrajectoryServer:: processJointInstruction axisGroup->syncTrapMove failed!!!轨迹添加失败";
     }
     TimeTool::getInstance()->recordOutTime(timer_id3[E_TIME3_C6]);

     pthread_mutex_unlock(&trajMutex);//test


     return tmpReturn;
 }


 int TrajectoryServer::processSingleAxisInstruction(MoveParameter moveParameter,
                                                    E_VELOCITY_TYPE velocityType)
 {
     printInformation(moveParameter,"processSingleAxisInstruction");


    QVector<int> axisIdVector;
    axisIdVector.resize(1);
    QVector<double> targetPoint;
    targetPoint.resize(1);

    axisIdVector[0]=moveParameter.singleAxisFlag;
    if(0>moveParameter.singleAxisFlag || jointsNum<=moveParameter.singleAxisFlag)
    {
        qDebug()<<"TrajectoryServer:: processSingleAxisInstruction moveParameter.singleAxisFlag error!!!"<< moveParameter.singleAxisFlag;
        addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5054,robotId,moveParameter.singleAxisFlag);
        return -102;
    }
    targetPoint[0]=moveParameter.targetPoint.positionValue[moveParameter.singleAxisFlag];


    if(moveParameter.velocityUnit!=ENUM_RATIO)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5012,robotId);
        return -101;
    }

     int tmpReturn=0;
     double tmpTime;
     pthread_mutex_lock(&trajMutex);//test
     TimeTool::getInstance()->recordInTime(timer_id3[E_TIME3_C6]);
     tmpReturn=axisGroup->syncZeroMove_velVariable(velocityType,targetPoint, moveParameter.velocity,
                axisIdVector,tmpTime,axisGroup->getMixDurationLeft()
                      ,moveParameter.cntRatio,moveParameter.lineNumber);

     if(1==tmpReturn)
     {
         addTrajTime(moveParameter.lineNumber,tmpTime);
     }
     TimeTool::getInstance()->recordOutTime(timer_id3[E_TIME3_C6]);
     pthread_mutex_unlock(&trajMutex);//test


     return tmpReturn;
 }


 int TrajectoryServer::addTrajTime(int lineNumber, double timeDuration)//调用者做锁保护
 {
     if(D_LINE_DEBUG)
     {
         qDebug()<<"addTrajTime,lineNumber"<<lineNumber<<" ,timeDuration"<<timeDuration;
     }
     lineQueue.enqueue(lineNumber);
     timeDurationQueue.enqueue(timeDuration);
     return 1;
 }

 int TrajectoryServer::resetTrajTime()
 {
     pthread_mutex_lock(&trajMutex);//test
     lineQueue.clear();
     timeDurationQueue.clear();
//     currentLine=ENUM_LINE_INITIAL;
     isCurrentLineFinished=false;
     currentLineRemainingTime=-1;
     currentLineTimeDuration=-1;
     lineTimeCount=0;
     lineTimeCountOld=-1;
     updateTrajStage=0;
     pthread_mutex_unlock(&trajMutex);//test
     return 1;
 }

 int TrajectoryServer::isMoveConditionOk_axis()
 {
     if(ENUM_TRAJECTORY_CEASED==basicStatus||ENUM_TRAJECTORY_STOPPED==basicStatus
             || ENUM_TRAJECTORY_INITIAL==basicStatus
             || 1==axisGroup->getErrorStatusFlag())
     {
         motionDebugInfo.realThreadStage=E_REAL_THREAD_MOVE_ONE_STEP_DISABLE;
         return -1;
     }

     //check servon state
//     if(false==isServoOn())
//     {
//         qDebug()<<"TrajectoryServer::moveOneStep() error occureed! false==isServoOn()";
//         addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5033,robotId);
//         motionDebugInfo.realThreadStage=E_REAL_THREAD_MOVE_ONE_STEP_DISABLE;
//         pthread_mutex_lock(&trajMutex);//test
//         coupleManagement->disableCouple();
//         pthread_mutex_unlock(&trajMutex);//test
//         return -1;
//     }
     return 1;
 }

 int TrajectoryServer::isMoveConditionOk_servo()
 {
     //check servon state
     if(ENUM_TRAJECTORY_RUN==basicStatus || ENUM_TRAJECTORY_STOPPING==basicStatus ||
                                             ENUM_TRAJECTORY_CEASING==basicStatus )
     {
         if(false==isServoOn())
         {
     //        qDebug()<<"TrajectoryServer::moveOneStep_carvingTp() error occureed! false==isServoOn()";
             addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5033,robotId);
             motionDebugInfo.realThreadStage=E_REAL_THREAD_MOVE_ONE_STEP_DISABLE;
             pthread_mutex_lock(&trajMutex);//test
             coupleManagement->disableCouple();
             pthread_mutex_unlock(&trajMutex);//test
             return -1;
         }
     }
     return 1;
 }
int TrajectoryServer::moveOneStep()
{
    if(1!=isMoveConditionOk_axis())
    {
        return -1;
    }

    TimeTool::getInstance()->recordInTime(timer_id3[E_TIME3_C1]);
//    timeAxisMoveOneStep();
    TimeTool::getInstance()->recordOutTime(timer_id3[E_TIME3_C1]);

    TimeTool::getInstance()->recordInTime(timer_id3[E_TIME3_C2]);


    int returnKey=axisGroupMoveOneStep();
    if(-2==returnKey)
    {
//       if(ENUM_TRAJECTORY_RUN==basicStatus)
//       {
//           basicStatus=ENUM_TRAJECTORY_TOSTOP;
//           addStopTraJectory();
//        }
        qDebug()<<"-2==returnKey quickStop()";
        quickStop();
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5005,robotId);
    }
    else if(1==returnKey)
    {
//        cameraScanTrigLoop();
    }
    if(1==axisGroup->getErrorStatusFlag())
    {
        qDebug()<<"1==axisGroup->getErrorStatusFlag()";
        quickStop();
    }
    else if(2==axisGroup->getErrorStatusFlag())
    {
//        qDebug()<<"2==axisGroup->getErrorStatusFlag()";
        addMsg(ENUM_MSG_WARNING,"MotionServer","trajectory",5072,robotId);
        allStop();
    }
    TimeTool::getInstance()->recordOutTime(timer_id3[E_TIME3_C2]);

//    if(-1==axisGroup->executeDataToMotor())
//    {
//        basicStatus=ENUM_TRAJECTORY_STOPPED;
//        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5006,robotId);
//    }

    TimeTool::getInstance()->recordInTime(timer_id3[E_TIME3_C3]);
    if(ENUM_TRAJECTORY_STOPPING==basicStatus && ENUM_TIME_AXIS_NORMAL==getTimeAxisStatus())
    {
        setBasicStatus(ENUM_TRAJECTORY_STOPPED,9990013);
        allowSingleAxisJogFlag=false;
//        resetAll(107);//如果电机跟随滞后，导致命令位置突变
//        if(1==axisGroup->isAllVelProfileFinished())
//        {
//            resetAll();
//        }
    }
    else if(ENUM_TRAJECTORY_CEASING==basicStatus && ENUM_TIME_AXIS_NORMAL==getTimeAxisStatus())
    {
        setBasicStatus(ENUM_TRAJECTORY_CEASED,9990014);
        allowSingleAxisJogFlag=false;
    }


//    updateTrajTime();

    TimeTool::getInstance()->recordOutTime(timer_id3[E_TIME3_C3]);

    TimeTool::getInstance()->recordInTime(timer_id3[E_TIME3_C4]);
//    //更新命令点
//    QVector<double> tmpJointsPosition;
//    axisGroup->getJointCommand(tmpJointsPosition);
//    for(int i=0;i<innerJointsNum;i++)
//    {
//        currentJointPoint_Command.positionValue[i]=tmpJointsPosition[i];
//    }
    TimeTool::getInstance()->recordOutTime(timer_id3[E_TIME3_C4]);


//    velocitySlowdownLoopRealtime();
    velocitySlowdownLoopRealtime_imdeat();

    motionDebugInfo.realThreadStage=E_REAL_THREAD_MOVE_ONE_STEP_OUT;
    return 1;
}

int TrajectoryServer::moveOneStep_carvingTp()
{
    //更新tp状态
//    motionDebugInfo.lineQueueSize;
//    if(ENUM_TRAJECTORY_CEASED==basicStatus||ENUM_TRAJECTORY_STOPPED==basicStatus
//            || ENUM_TRAJECTORY_INITIAL==basicStatus
//            || 1==axisGroup->getErrorStatusFlag())
//    {
//        motionDebugInfo.realThreadStage=E_REAL_THREAD_MOVE_ONE_STEP_DISABLE;
//        return -1;
//    }


    int tmpTpKey=carvingModule->loopRealTimeOnce();
    switch(tmpTpKey)
    {
    case -100:
    {
//        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5086,robotId);
        break;
    }
    }

    pthread_mutex_lock(&trajMutex);//test
#ifdef D_USE_JOINT_INTERPOLATION
    std::vector<double> jointCommandOut;
    carvingModule->getJointCommand(jointCommandOut);
    for(int j=0;j<jointCommandOut.size();j++)
    {
        axisGroup->moveOneStepByPosition(jointCommandOut[j],j);

    }
//    qDebug()<<"tp moveOneStepByPosition "<<jointCommandOut[0]<<jointCommandOut[1]<<jointCommandOut[2]
//              <<jointCommandOut[3]<<jointCommandOut[4]<<jointCommandOut[5];

#else
    PointPro tmpPoint=currentCoordinatePoint_Feedback;
    carvingModule->getCoarsePositionCommand(tmpPoint.positionValue);

//        Rotation tmpOrient=Rotation::Quaternion(tmpPoint.positionValue[3],tmpPoint.positionValue[4],
//                tmpPoint.positionValue[5],tmpPoint.positionValue[6]);
//        tmpOrient.GetRPY(tmpPoint.positionValue[3],tmpPoint.positionValue[4],tmpPoint.positionValue[5]);
//        tmpPoint.positionValue[3]=tmpPoint.positionValue[3]/M_PI*180.0;
//        tmpPoint.positionValue[4]=tmpPoint.positionValue[4]/M_PI*180.0;
//        tmpPoint.positionValue[5]=tmpPoint.positionValue[5]/M_PI*180.0;

    axisGroup->moveOneStep_byUserCoordinateCommand(tmpPoint);
#endif
    pthread_mutex_unlock(&trajMutex);//test

//    velocitySlowdownLoopRealtime(true);//tp规划没有关节速度限制。

    motionDebugInfo.realThreadStage=E_REAL_THREAD_MOVE_ONE_STEP_OUT;
    return 1;
}

int TrajectoryServer::moveOneStep_simpleTp()
{
    pthread_mutex_lock(&trajMutex);
    axisGroup->axisSimpleLoopOnce();
    pthread_mutex_unlock(&trajMutex);
}

int TrajectoryServer::moveOneStepByTorque(double torqueIn, int axisId)
{
    return axisGroup->moveOneStepByTorque(torqueIn,axisId);//coredump
}

int TrajectoryServer::moveOneStepByPosition_plus(double positionPlus, int axisId)
{
     return axisGroup->moveOneStepByPosition_plus(positionPlus,axisId);//coredump
}

int TrajectoryServer::timeAxisMoveOneStep()
{
    motionDebugInfo.realThreadStage=E_REAL_THREAD_TIME_AXIS_MOVE_ONE_STEP;
    int returnResult;
    QVector<double> timeAxisScaleOut;
    pthread_mutex_lock(&trajMutex);//test
    returnResult=axisGroup->timeAxisMoveOneStep(timeAxisScaleOut);
    carvingModule->setTimeAxisScale(timeAxisScaleOut[0]);
    pthread_mutex_unlock(&trajMutex);//test
    motionDebugInfo.timeAxisScale0=timeAxisScaleOut[0];
    return returnResult;
}

int TrajectoryServer::axisGroupMoveOneStep()
{
    motionDebugInfo.realThreadStage=E_REAL_THREAD_AXIS_GROUP_MOVE_ONE_STEP;
    int returnResult;
    pthread_mutex_lock(&trajMutex);//test
//    returnResult=axisGroup->moveOneStep();
    returnResult=axisGroup->moveOneStep_cnt();
    //不需要需要保证锁的完整性。
//    waveMoveOneStep();//叠加摆动焊接轨迹位置。
    beltTrackLoop();
//    // 根据混合后的关节位置，反求世界位置。
//    axisGroup->setPositionFrameCommand_byJointCommand(); 导致trackStart过渡逻辑需要修改。
    pthread_mutex_unlock(&trajMutex);//test



    return returnResult;
}


int TrajectoryServer::calculatePointByTrajectory(KDL::Frame &frameReturn,int &lineNumber,double &timeRemainning)
{

}

int TrajectoryServer::writeDataToFile()
{
    axisGroup->writeDataToFile();
}

int TrajectoryServer::waveMoveOneStep()
{
//    Frame positionFrameCommandBefore,positionFrameCommandAfter;
////    axisGroup->getFinishedAxisPositionCommand(positionFrameCommandBefore);
//    calculateCurrentCommandPoint_byJointCommand(positionFrameCommandBefore);
//    if(1!=waveMotionControl->moveOneStep(positionFrameCommandBefore,positionFrameCommandAfter))
//    {
//        return 0;
//    }
//    PointPro tmpPoint=coordinateManager->frameToPointPro_CART(positionFrameCommandAfter);
//    axisGroup->moveOneStep_byUserCoordinateCommand(tmpPoint);
    return 1;
}



int TrajectoryServer::getStatus()
{
    return basicStatus;
}

int TrajectoryServer::getCurrentJointPoint_Feedback(PointPro &jointPoint)
{
    pthread_mutex_lock(&trajMutex);//test
    jointPoint=currentJointPoint_Feedback;
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}

int TrajectoryServer::getCurrentCoordinatePoint_Feedback( PointPro &coordinatePoint)
{
    pthread_mutex_lock(&trajMutex);//test
    coordinatePoint=currentCoordinatePoint_Feedback;
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}

int TrajectoryServer::getCurrentCoordinatePoint_Command(PointPro &coordinatePoint)
{
    //todo currentCoordinatePoint_Command数据没有更新
    qDebug()<<"not support :getCurrentCoordinatePoint_Command";
    return 0;
//    pthread_mutex_lock(&getMutex);
//    coordinatePoint=currentCoordinatePoint_Command;
//    pthread_mutex_unlock(&getMutex);
//    return 1;
}
//hualei
int TrajectoryServer::calculateCurrentPoint()
{

    pthread_mutex_lock(&trajMutex);//test
    //根据脉冲计算关节点
    QVector<double> jointsPositionTmp;
    QVector<double> jointsVelOutTmp;
    QVector<double> jointsAccOutTmp;
    QVector<double> jointsTorqueOutTmp;
    axisGroup->upDateAllFeedbackAndOffset();
    axisGroup->getJointFeedback(jointsPositionTmp,jointsVelOutTmp,jointsAccOutTmp,jointsTorqueOutTmp);

    currentJointPoint_Feedback.pointNum=0;
    currentJointPoint_Feedback.recordStatus=1;
    currentJointPoint_Feedback.jointFlag=JOINT_FLAG_JOINT;//
    currentJointPoint_Feedback.ufValue=getUserIndex();
    currentJointPoint_Feedback.utValue=getToolIndex();
    for(int i=0;i<innerJointsNum;i++)
    {
        currentJointPoint_Feedback.positionValue[i]=jointsPositionTmp[i];
    }

    //计算3d相机坐标
//    PointPro tmpInbase;
//    coordinateManager->getWeldPositionByCameraTransfer(currentCoordinatePoint_Feedback,originCameraPoint_x,originCameraPoint_y
//                                                       ,0,tmpInbase);
//    cameraPointInBase_x=tmpInbase.positionValue[0];
//    cameraPointInBase_y=tmpInbase.positionValue[1];
//    cameraPointInBase_z=tmpInbase.positionValue[2];
    pthread_mutex_unlock(&trajMutex);//test

    //根据关节计算坐标点
    if(1!=pointCoordinateToCartesian(currentJointPoint_Feedback,currentCoordinatePoint_Feedback))
    {
        qDebug()<<"----------error pointCoordinateToCartesian5059";
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5059,robotId);
        return -1;
    }


//    Frame tmpFrame;
//    if(1!=pointProToFrame(currentCoordinatePoint_Feedback,tmpFrame))
//    {
//        qDebug()<<"calculateCurrentPoint error ";
//    }
//    currentFrame_Feedback[0]=tmpFrame;



//    //更新ＨＭＩ需要的点（缓存处理，防止算法被ＨＭＩ抢占阻塞）
//    if(0==pthread_mutex_trylock(&getMutex))
//    {
//        currentJointPoint_FeedbackOld=currentJointPoint_Feedback;
//        currentCoordinatePoint_FeedbackOld=currentCoordinatePoint_Feedback;
//        pthread_mutex_unlock(&getMutex);
//    }


    return 1;
}

int TrajectoryServer::calculateCurrentLaserPoint()
{
    pthread_mutex_lock(&trajMutex);
    //计算3d相机坐标
    PointPro tmpInbase;
    coordinateManager->getWeldPositionByCameraTransfer(currentCoordinatePoint_Feedback,originCameraPoint_x,originCameraPoint_y
                                                       ,0,tmpInbase);
    cameraPointInBase_x=tmpInbase.positionValue[0];
    cameraPointInBase_y=tmpInbase.positionValue[1];
    cameraPointInBase_z=tmpInbase.positionValue[2];
    pthread_mutex_unlock(&trajMutex);
    return 1;
}

int TrajectoryServer::calculateCurrentCommandPoint_byJointCommand(Frame &positionFrameCommandOut)
{
    return axisGroup->calculateCurrentFrameCommandPoint_byJointCommand(positionFrameCommandOut);

}

int TrajectoryServer::setPositionFrameCommand_byJointCommand()
{
    return axisGroup->setPositionFrameCommand_byJointCommand();

}

int TrajectoryServer::synchronizeMotorFeedbackToCalculatePosition()
{
    PointPro jointPoint;
    PointPro coordinatePoint;
    calculateCurrentPoint();
    getCurrentJointPoint_Feedback(jointPoint);
    getCurrentCoordinatePoint_Feedback(coordinatePoint);
    pthread_mutex_lock(&trajMutex);//test
    axisGroup->clearTraj(true);
    carvingModule->emcTpClear();
    carvingModule->resetCurrentPosition(currentCoordinatePoint_Feedback.positionValue
                                        ,currentJointPoint_Feedback.positionValue);
    int tmpSize=currentCoordinatePoint_Feedback.positionValue.size();
    for(int i=0;i<tmpSize;i++)
    {
        tpBeforePose[i]=currentCoordinatePoint_Feedback.positionValue[i];
    }
    pthread_mutex_unlock(&trajMutex);//test
//    pthread_mutex_lock(&getMutex);
//    currentJointPoint_Command=currentJointPoint_Feedback;
//    currentCoordinatePoint_Command=currentCoordinatePoint_Feedback;
//    pthread_mutex_unlock(&getMutex);
    return 1;
}


int TrajectoryServer::synchronizeMotorFeedbackToCalculatePosition(int axisIndex)
{
    PointPro jointPoint;
    PointPro coordinatePoint;
    calculateCurrentPoint();
    getCurrentJointPoint_Feedback(jointPoint);
    getCurrentCoordinatePoint_Feedback(coordinatePoint);
    pthread_mutex_lock(&trajMutex);//test
    axisGroup->clearTraj_byAxisId(axisIndex,true);
    pthread_mutex_unlock(&trajMutex);//test
//    pthread_mutex_lock(&getMutex);
//    currentJointPoint_Command=currentJointPoint_Feedback;
//    currentCoordinatePoint_Command=currentCoordinatePoint_Feedback;
//    pthread_mutex_unlock(&getMutex);
    return 1;
}

int TrajectoryServer::syncAxisPoseCommandToCarvingModule()
{

    PointPro coordinatePoint;
    QVector<double> jointsPositionOut;
    vector<double> vejointsPositionOut;
    pthread_mutex_lock(&trajMutex);//test
    axisGroup->getJointCommand(jointsPositionOut);
    axisGroup->calculateCurrentFrameCommandPoint_byJointCommand(coordinatePoint);//根据关节算笛卡尔坐标
    axisGroup->clearTraj(false);
    carvingModule->emcTpClear();
    vejointsPositionOut=jointsPositionOut.toStdVector();
    carvingModule->resetCurrentPosition(coordinatePoint.positionValue
                                        ,vejointsPositionOut);
    int tmpSize=coordinatePoint.positionValue.size();
    for(int i=0;i<tmpSize;i++)
    {
        tpBeforePose[i]=coordinatePoint.positionValue[i];
    }
    pthread_mutex_unlock(&trajMutex);//test
//    qDebug()<<"******synced tpBeforePose"<<tpBeforePose[3]<<tpBeforePose[4]<<tpBeforePose[5];


    return 1;
}


//int  TrajectoryServer::getCalibrateMotorValueOffset(QVector<int> &pulseValue)
//{
//    pulseValue.resize(jointsNum);
//    for(int i=0;i<jointsNum;i++)
//    {
//        pulseValue[i]=0;
//    }

//    return 1;
//}

QVector<MotorData> TrajectoryServer::getMotorCommandDataVector()
{
    pthread_mutex_lock(&trajMutex);//test
    int result=axisGroup->executeDataToMotor();
    pthread_mutex_unlock(&trajMutex);//test
    if(-1==result)
    {
        setBasicStatus(ENUM_TRAJECTORY_STOPPED,9990015);
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory",5006,robotId);
    }
    pthread_mutex_lock(&trajMutex);//test
    QVector<MotorData> tmpReturn=axisGroup->getMotorCommandDataVector();
    pthread_mutex_unlock(&trajMutex);//test
    motionDebugInfo.realThreadStage=E_REAL_THREAD_OUT;
    return tmpReturn;
}

QVector<MotorData> TrajectoryServer::getMotorFeedbackDataVector()
{
    return axisGroup->getMotorFeedbackDataVector();
}



int TrajectoryServer::setMotorFeedbackDataVector(QVector<MotorData> motorFeedbackIn)
{
    if(motorFeedbackIn.size()< jointsNum )
    {
        qDebug()<<"TrajectoryServer::setMotorFeedbackDataVector(QVector<MotorData> motorFeedbackIn) error";
        return -1;
    }
    TimeTool::getInstance()->recordInTime(timer_id3[E_TIME3_C5]);
    pthread_mutex_lock(&trajMutex);//test
    TimeTool::getInstance()->recordOutTime(timer_id3[E_TIME3_C5]);
    TimeTool::getInstance()->recordInTime(timer_id3[E_TIME3_C10]);
    axisGroup->setMotorFeedbackDataVector(motorFeedbackIn);
    TimeTool::getInstance()->recordOutTime(timer_id3[E_TIME3_C10]);
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}

int TrajectoryServer::setMianControlRatio(double ratioIn)
{
    mainStatusRatio=ratioIn;
    return 1;
}

int TrajectoryServer::setControlSpeedRatio(double controlSpeedRatioIn, double time)
{

    if(1==roboParameter->isAgvMotionOn)
    {
        virtualLineControl->setControlSpeedRatio(controlSpeedRatioIn,time);
    }
    if(1==roboParameter->isArmMotionOn)
    {
        controlSpeedRatio=controlSpeedRatioIn;
        double totalSpeedRatio=controlSpeedRatio*pidSpeedRatio*safeSpeedRatio*adjustSpeedRatio*mainStatusRatio;
        if(D_STOP_SPEED_RATIO>totalSpeedRatio)
        {
            totalSpeedRatio=0.0;
        }
        setTimeAxisScale(totalSpeedRatio,time);
        carvingModule->emcSetVelLimit(roboParameter->lineMaxSpeed*totalSpeedRatio);
        carvingModule->emcSetRotateLimit(roboParameter->rotate_eqradius*totalSpeedRatio);
    }
    switch(motionModel)
    {
        case ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS:
        {

        }
        break;
        case ENUM_MOTION_MODEL_DRAG:
        {

        }
        break;
        case ENUM_MOTION_MODEL_HOMING:
        {

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK:
        {

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK:
        {


        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_JOG:
        {

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL:
        {

        }
        break;

        default:
        {

        }
    }
    return 1;
}

int TrajectoryServer::setSafeSpeedRatio(double safeSpeedRatioIn,double time)
{
    if(1==roboParameter->isAgvMotionOn)
    {
        virtualLineControl->setSafeSpeedRatio(safeSpeedRatioIn,time);
    }
    if(1==roboParameter->isArmMotionOn)
    {
        safeSpeedRatio=safeSpeedRatioIn;
        double totalSpeedRatio=controlSpeedRatio*pidSpeedRatio*safeSpeedRatio*adjustSpeedRatio*mainStatusRatio;
        if(D_STOP_SPEED_RATIO>totalSpeedRatio)
        {
            totalSpeedRatio=0.0;
        }
        setTimeAxisScale(totalSpeedRatio,time);
        carvingModule->emcSetVelLimit(roboParameter->lineMaxSpeed*totalSpeedRatio);
        carvingModule->emcSetRotateLimit(roboParameter->rotate_eqradius*totalSpeedRatio);
    }

    switch(motionModel)
    {
        case ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS:
        {

        }
        break;
        case ENUM_MOTION_MODEL_DRAG:
        {

        }
        break;
        case ENUM_MOTION_MODEL_HOMING:
        {

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK:
        {

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK:
        {


        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_JOG:
        {

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL:
        {

        }
        break;

        default:
        {

        }
    }
    return 1;
}

int TrajectoryServer::setAdjustSpeedRatio(double ratioIn, double time)
{

    if(1==roboParameter->isArmMotionOn)
    {
        adjustSpeedRatio=ratioIn;
        double totalSpeedRatio=controlSpeedRatio*pidSpeedRatio*safeSpeedRatio*adjustSpeedRatio*mainStatusRatio;
        if(D_STOP_SPEED_RATIO>totalSpeedRatio)
        {
            totalSpeedRatio=0.0;
        }
        setTimeAxisScale(totalSpeedRatio,time);
//        carvingModule->emcSetVelLimit(roboParameter->lineMaxSpeed*totalSpeedRatio);
//        carvingModule->emcSetRotateLimit(roboParameter->rotate_eqradius*totalSpeedRatio);
    }

    switch(motionModel)
    {
        case ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS:
        {

        }
        break;
        case ENUM_MOTION_MODEL_DRAG:
        {

        }
        break;
        case ENUM_MOTION_MODEL_HOMING:
        {

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK:
        {

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK:
        {


        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_JOG:
        {

        }
        break;
        case ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL:
        {

        }
        break;

        default:
        {

        }
    }
    return 1;
}


int TrajectoryServer::getTimeAxisStatus(int timeAxisIndex)
{
    if(1==axisGroup->isTimeVelProfileEmpty(timeAxisIndex))
    {
       return ENUM_TIME_AXIS_NORMAL;
    }
    else
    {

    }
    return ENUM_TIME_AXIS_DESCEND;
}

int TrajectoryServer::getArmMovingFlag(int &lineIdOut, E_MOVING_FLAG &extFlagOut)
{
   lineIdOut=armRunningLineId;
   extFlagOut=armRunningFlag;
   return 1;

}

int TrajectoryServer::updateArmMovingFlag(int &lineIdOut, E_MOVING_FLAG &extFlagOut)
{
    double tmpIdf=getRunningNumber();
    lineIdOut=tmpIdf;
    double tmpDiff=tmpIdf-lineIdOut;
    if(tmpDiff>0.8)
    {
        if(1==isTrajectoryEmpty())
        {
            if(1==isTargetOffsetCheckOk())
            {
                extFlagOut=E_MOVING_FLAG_REACHED;
                return 1;
            }
        }
    }
    else if(tmpDiff<0.4)
    {
    }
    else
    {

    }
    extFlagOut=E_MOVING_FLAG_RUNNING;
    return 1;
}

int TrajectoryServer::getTimeAxisStatus()
{
    if(1==axisGroup->isTimeVelProfileEmpty())
    {
       timeAxisStatus=ENUM_TIME_AXIS_NORMAL;
    }
    return timeAxisStatus;
}

int TrajectoryServer::isTargetOffsetCheckOk()
{
    pthread_mutex_lock(&trajMutex);//test
    int returnResult=axisGroup->isTargetOffsetCheckOk();
    pthread_mutex_unlock(&trajMutex);//test
    return returnResult;
}



int TrajectoryServer::isTrajectoryEmpty()
{
   int tmpKey;
   pthread_mutex_lock(&trajMutex);//test
   tmpKey=isTrajectoryEmpty_nolock();
   pthread_mutex_unlock(&trajMutex);//test
   return tmpKey;
}

int TrajectoryServer::isTrajectoryEmpty_nolock()
{

    int returnResult;
    if(isInCarvingTp)
    {
        if(0==carvingModule->getTpQueueSize())
        {
            returnResult=1;
        }
        else
        {
            returnResult=0;
        }
    }
    else
    {
        QVector<int> axisIdVector;
        axisIdVector.resize(jointsNum);
        for(int i=0;i<jointsNum;i++)
        {
            axisIdVector[i]=i;
        }


    //    int commandQueueSize=commandQueue.size();//不能判断commandQueueSize，否则跟踪会与８个左右周期位置不变，导致速度降为０
        if( 1==axisGroup->isAllVelProfileFinished(axisIdVector))
        {
            returnResult= 1;
        }
        else
        {
            returnResult= 0;
        }
    }


    return returnResult;
}


bool TrajectoryServer::isInFineMoving()
{
    bool returnResult=axisGroup->isInFineMoving();
    return returnResult;
}




QVector<ToolCoordinateInfo> TrajectoryServer::getToolCoordinateList()
{
    QVector<ToolCoordinateInfo> tmpCoordinateList;
//    if(basicStatus!=ENUM_TRAJECTORY_STOPPED)
//    {
//        addMsg(ENUM_MSG_WARNING,"MotionServer","trajectory",5056,robotId);
//        qDebug()<<"getToolCoordinateList error, basicStatus!=ENUM_TRAJECTORY_STOPPED ";
//        return tmpCoordinateList;
//    }
    pthread_mutex_lock(&trajMutex);//test
    tmpCoordinateList=coordinateManager->getToolCoordinateList();
    pthread_mutex_unlock(&trajMutex);//test
    return tmpCoordinateList;
}

QVector<UserCoordinateInfo> TrajectoryServer::getUserCoordinateList()
{
    QVector<UserCoordinateInfo> tmpCoordinateList;
//    if(basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED)
//    {
//        qDebug()<<"getUserCoordinateList error, basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED.";
//        return tmpCoordinateList;
//    }
    pthread_mutex_lock(&trajMutex);//test
    tmpCoordinateList=coordinateManager->getUserCoordinateList();
    pthread_mutex_unlock(&trajMutex);//test
    return tmpCoordinateList;

}

int TrajectoryServer::getToolIndex()
{
    return coordinateManager->getCurrentToolIndex();
}

int TrajectoryServer::getUserIndex()
{
    return coordinateManager->getCurrentUserIndex();
}

int TrajectoryServer::getJointNum()
{
    return jointsNum;
}

float TrajectoryServer::getJointMaxPosition(int jointIndex)
{

    pthread_mutex_lock(&getMutex);
    float tmpReturn= roboParameter->jointAttributeVector[jointIndex].maxPosition;
    pthread_mutex_unlock(&getMutex);
    return tmpReturn;

}

float TrajectoryServer::getJointMinPosition(int jointIndex)
{
    pthread_mutex_lock(&getMutex);
    float tmpReturn= roboParameter->jointAttributeVector[jointIndex].minPosition;
    pthread_mutex_unlock(&getMutex);
    return tmpReturn;

}

int TrajectoryServer::getJointType(int jointIndex)
{
    pthread_mutex_lock(&getMutex);
    int tmpReturn= roboParameter->jointAttributeVector[jointIndex].jointType;
    pthread_mutex_unlock(&getMutex);
    return tmpReturn;

}

int TrajectoryServer::getAxisLogicalType(int jointIndex)
{
    pthread_mutex_lock(&getMutex);
    int tmpValue=roboParameter->jointAttributeVector[jointIndex].axisLogicalType;
    pthread_mutex_unlock(&getMutex);
    return tmpValue;
}

int TrajectoryServer::getAxisInformation(std::vector<AxisLimit> &axisLimit, std::vector<AxisLimit> &extAxisLimit,
                                         std::vector<int> &axisUnit, std::vector<int> &extAxisUnit,
                                         std::vector<int> &axisType, std::vector<int> &extAxisType)
{
    pthread_mutex_lock(&getMutex);
    axisLimit.resize(jointsNum);
    axisUnit.resize(jointsNum);
    axisType.resize(jointsNum);
    for(int i=0;i<roboParameter->jointAttributeVector.size();i++)
    {
        axisLimit[i].negative=roboParameter->jointAttributeVector[i].minPosition;
        axisLimit[i].positive=roboParameter->jointAttributeVector[i].maxPosition;
        axisUnit[i]=roboParameter->jointAttributeVector[i].jointType;
        axisType[i]=roboParameter->jointAttributeVector[i].axisLogicalType;
    }
    pthread_mutex_unlock(&getMutex);
    return 1;

}

void TrajectoryServer::getSoftLimit(std::vector<std::vector <float> >  &axisLimit, std::vector<int> &axisUnit)
{
    axisLimit.resize(jointsNum);
    axisUnit.resize(jointsNum);
    for(int i=0;i<jointsNum;i++)
    {
        axisLimit[i].resize(2);
        axisLimit[i][0]=roboParameter->jointAttributeVector[i].minPosition;
        axisLimit[i][1]=roboParameter->jointAttributeVector[i].maxPosition;
        axisUnit[i]=roboParameter->jointAttributeVector[i].jointType;
    }
}


int TrajectoryServer::activateToolCoordinate(int index)
{
    pthread_mutex_lock(&trajMutex);//test
    coordinateManager->activateToolCoordinate(index);
    setPositionFrameCommand_byJointCommand();
    pthread_mutex_unlock(&trajMutex);//test
    calculateCurrentPoint();
    return 1;
}

int TrajectoryServer::activateUserCoordinate(int index)
{
    pthread_mutex_lock(&trajMutex);//test
    coordinateManager->activateUserCoordinate(index);
    setPositionFrameCommand_byJointCommand();
    pthread_mutex_unlock(&trajMutex);//test
    calculateCurrentPoint();
    return 1;
}

int TrajectoryServer::setToolCoordinate(ToolCoordinateInfo& toolCoordinate)
{
//    if(basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED)
//    {
//        qDebug()<<"setToolCoordinate error, basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED.";
//        return -1;
//    }

    if(ENUM_TOOL_20POINTS_ZEROCAL==toolCoordinate.method)
    {
        vector<double> zeroOffsetOut;
        coordinateManager->setToolCoordinate_20pOfZero(toolCoordinate,zeroOffsetOut);
    }
    else
    {
        coordinateManager->setToolCoordinate(toolCoordinate);
    }


    return 1;
}

int TrajectoryServer::pointCoordinateToJoint(const PointPro &pointIn, PointPro &jointPointOut,
                                             bool isErrorNotice)
{
     //运动命令点类型转换都掉用本函数。ｈmi不使用
    jointPointOut=pointIn;
    int returnKey;
    pthread_mutex_lock(&trajMutex);//test
    vector<double> jointInitialList;
    if(jointPointOut.configString.empty())//如果没有fut字符则，默认使用当前角度进行初始求解。
    {
        jointInitialList.resize(innerJointsNum);
        for(int i=0;i<innerJointsNum;i++)
        {
            jointInitialList[i]=axisGroup->axisArry[i]->positionCommand;
        }
    }
    returnKey=coordinateManager->pointCoordinateToJoint(pointIn,jointInitialList,jointPointOut
                                                        ,isErrorNotice);
    pthread_mutex_unlock(&trajMutex);//test
//    printPointPro(tmpPoint,"tmpPoint2 pointCoordinateToJoint");
    return returnKey;

}

int TrajectoryServer::pointCoordinateToJoint_noDeviationCheck(const PointPro &pointIn,
                                PointPro &jointPointOut,bool isErrorNotice)
{
    //hmi上的点类型转换都掉用本函数
    jointPointOut=pointIn;
    int returnKey;
    pthread_mutex_lock(&trajMutex);//test
    vector<double> jointInitialList;//不给初始角度
    if(jointPointOut.configString.empty())//如果没有fut字符则，默认使用当前角度进行初始求解。
    {
        jointInitialList.resize(innerJointsNum);
        for(int i=0;i<innerJointsNum;i++)
        {
            jointInitialList[i]=axisGroup->axisArry[i]->positionCommand;
        }
    }
    returnKey=coordinateManager->pointCoordinateToJoint(pointIn,jointInitialList,jointPointOut
                                                        ,isErrorNotice);
    pthread_mutex_unlock(&trajMutex);//test
//    printPointPro(tmpPoint,"tmpPoint2 pointCoordinateToJoint");
    return returnKey;
}

int TrajectoryServer::pointCoordinateToCartesian(const PointPro pointIn, PointPro &pointOut)
{
    pthread_mutex_lock(&trajMutex);//test
    int tmpKey=coordinateManager->pointCoordinateToCartesian(pointIn,pointOut);
    pthread_mutex_unlock(&trajMutex);//test
//    printPointPro(tmpPoint,"tmpPoint1 pointCoordinateToCartesian");
    return tmpKey;

}

int TrajectoryServer::pointProToFrame(const PointPro& pointPro,Frame &frameOut)
{
    pthread_mutex_lock(&trajMutex);//test
//    tmpFrame=coordinateManager->pointProToFrame(pointPro);
    int tmpKey=coordinateManager->pointProToFrame_noUT(pointPro,frameOut);
    pthread_mutex_unlock(&trajMutex);//test

    return tmpKey;
}

PointPro TrajectoryServer::frameToPointPro_CART(const Frame& f)
{
    PointPro tmpPoint;
    pthread_mutex_lock(&trajMutex);//test
    tmpPoint=coordinateManager->frameToPointPro_CART(f);
    pthread_mutex_unlock(&trajMutex);//test

    return tmpPoint;
}

int TrajectoryServer::printPointPro(PointPro point, QString title)
{
    qDebug()<<"----------------------------"<<title;
    qDebug()<<"jointFlag:"<<point.jointFlag;
    qDebug()<<"pointNum:"<<point.pointNum;
    qDebug()<<"groupNum:"<<point.groupNum;
    qDebug()<<"ufValue:"<<point.ufValue;
    qDebug()<<"utValue:"<<point.utValue;
    qDebug()<<"recordStatus:"<<point.recordStatus;
    qDebug()<<"configString:"<<QString::fromStdString(point.configString);
    for(int i=0;i<point.configInt.size();i++)
    {
       qDebug()<<"configInt:"<<point.configInt[i]<< ", ";
    }

    for(int i=0;i<point.positionValue.size();i++)
    {
       qDebug()<<"positionValue:"<<point.positionValue[i]<< ", ";
    }

    for(int i=0;i<point.extPositionValue.size();i++)
    {
       qDebug()<<"extPositionValue:"<<point.extPositionValue[i]<< ", ";
    }

    qDebug()<<"-------------------";

    return 1;

}

int TrajectoryServer::setUserCoordinate(UserCoordinateInfo& userCoordinate)
{
//    if(basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED)
//    {
//        qDebug()<<"setUserCoordinate error, basicStatus!=ENUM_TRAJECTORY_STOPPED && basicStatus==ENUM_TRAJECTORY_CEASED.";
//        return -1;
//    }

    coordinateManager->setUserCoordinate(userCoordinate);

    return 1;
}

float TrajectoryServer::getRunningNumber()
{
    if(isInCarvingTp)
    {
        return carvingModule->getRunningNumber();
    }
    return axisGroup->getRunningNumber();
}

int TrajectoryServer::setRunningNumber_robotArmControl(float numberIn)
{
//    currentLine=numberIn;
    carvingModule->setRunningNumber_robotArmControl(numberIn);
    return axisGroup->setRunningNumber_robotArmControl(numberIn);
}

float TrajectoryServer::getRunningNumber_virtualLineControl()
{
    return virtualLineControl->getRunningNumber();
}

int TrajectoryServer::setRunningNumber_virtualLineControl(float numberIn)
{
     return virtualLineControl->setRunningNumber_virtualLineControl(numberIn);
}

double TrajectoryServer::getBarrierProtectRatioOfMoving()
{
    return virtualLineControl->getBarrierProtectRatioOfMoving();
}

int TrajectoryServer::setPathMovingInfo_virtualLineControl(PathMovingInfo pathInfoIn)
{
    return virtualLineControl->setPathMovingInfo_virtualLineControl(pathInfoIn);
}

int TrajectoryServer::getPathMovingInfo_virtualLineControl(PathMovingInfo &infoOut)
{
    return virtualLineControl->getPathMovingInfo_virtualLineControl(infoOut);
}

//int TrajectoryServer::getLineTimeDuration()
//{
//    return currentLineTimeDuration;
//}

//int TrajectoryServer::getLineRemainingTime()
//{
////    return currentLineRemainingTime;
//    return axisGroup->getLineRemainingTime();
//}
int TrajectoryServer::trajMutexFlag()
{
    return 0;//只要锁里面没有文件或设备的操作，都会很快就会结束锁（没有运算时间超过1s的，坐标系计算和设定使用独立的锁，计算和文件存储分开）。
    int returnFlag=pthread_mutex_trylock(&trajMutex);//不能用trylock，否则ethercat更新会有问题。
    if(0==returnFlag)
    {
       pthread_mutex_unlock(&trajMutex);//test
    }
    return returnFlag;
}
int TrajectoryServer::getFeedbackJoint(QVector<double> &jointsPosition, QVector<double> &jointsVel,
                                       QVector<double> &jointAcc, QVector<double> &jointTorque)
{
    pthread_mutex_lock(&trajMutex);//test
    axisGroup->getFeedbackJoint(jointsPosition,jointsVel,jointAcc,jointTorque);
    pthread_mutex_unlock(&trajMutex);//test
    return 1;

}

int TrajectoryServer::getAgvSteeringPositionFeedback(QVector<double> &positionOut)
{
    pthread_mutex_lock(&trajMutex);//test
    axisGroup->getAgvSteeringPositionFeedback(positionOut);
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}

int TrajectoryServer::algorithmFunctioncall(const QVector<bool> &servoOnStatus, const QVector<MotorData> &motorFeedback,
                                            QVector<MotorData> &motorCommandOut,int &returnMutexFlag
                                            ,QVector<E_MOTOR_CONTROL_MODE> &controlModeOut)
{
    //轨迹规划采用阻塞方式工作，每次只处理一条运动指令（平滑段算一条），然后等待运行到达后才处理第２条。如果要预处理多条，机制会比较复杂。
    motorCommandOut.clear();
    if(false==isMotorCommandValid)
    {
        returnMutexFlag=-1;
        return 0;
    }
    //如果算法线程占用trajMutex,则跳出循环，发上一次的电机命令。
    motionDebugInfo.realThreadStage=E_REAL_THREAD_IN;
    returnMutexFlag=trajMutexFlag();
    //
    getMotorControlMode(controlModeOut);
    if(0!=returnMutexFlag)
    {
//        QVector<MotorData> tmpReturn;
        motionDebugInfo.realThreadStage=E_REAL_THREAD_MUTEX_FAILED;
        return 0;
    }

   TimeTool::getInstance()->recordInTime(timer_id2[E_TIME2_C1]);
   //更新ethercat电机反馈值到关节点、坐标点。
   //hualei ,need todo , cost to much time,sometime more than 1.2ms.
   setMotorFeedbackDataVectorFromEthercat_UpdateCurrentJointPointCoordinatePoint(motorFeedback);
   TimeTool::getInstance()->recordOutTime(timer_id2[E_TIME2_C1]);


   TimeTool::getInstance()->recordInTime(timer_id2[E_TIME2_C2]);

    setServoOnStatus(servoOnStatus);

//    axisPositionCommandLimitApproachCheck();放在这里会因为positionCommandOld=positionCommand判断失效

//    axisFeedbackCheck();

    motionServerLoopRealTime();//time test

    TimeTool::getInstance()->recordOutTime(timer_id2[E_TIME2_C2]);

    TimeTool::getInstance()->recordInTime(timer_id2[E_TIME2_C3]);
    if(recordWareFlag)
    {
        recordDataToWaveBuffer();//耗时约50us
    }

    TimeTool::getInstance()->recordOutTime(timer_id2[E_TIME2_C3]);

    motionDebugInfo.realThreadStage=E_REAL_THREAD_MOVE_ONE_STEP_GET_JOINT_COMMAND;
    motorCommandOut=getMotorCommandDataVector();
//    qDebug()<<"1112motorCommandOut"<<motorCommandOut[1].position<<"servoOnStatus[1]"<<servoOnStatus[1];
    axisPositionCommandLimitApproachCheck();
    axisFeedbackCheck();
    return 1;
}

#include <unistd.h>
#include <sys/syscall.h>
#define gettid() syscall(__NR_gettid)

void* TrajectoryServer::threadRun(void *)
{
    printf("TrajectoryServer::threadRun theread start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 40;
    sched_setscheduler(0,SCHED_FIFO,&param);
    qDebug()<<"************************TrajectoryServer commonthread pid="<<gettid();
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
//    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
//        qDebug("mlockall failed");
//    }
//    clock_gettime( CLOCK_MONOTONIC, &cycleTimespec );

   // qDebug()<<"this=="<<this;
   // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); //允许退出线程
    //pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); //设置立即取消

    // 等待急停　di　模块初始化
    usleep(3000000);
    while(1)
    {
       // pthread_testcancel();/*the thread can be killed only here*/
         usleep(1000000);
        while(1)
        {
            usleep(D_COMMON_LOOP_PERIOD);
            motionServerLoopCommon();
        }
    }
    printf("TrajectoryServer thread quit!!!");
}

void* TrajectoryServer::threadTask( void* classPtr )
{
    qDebug()<<"TrajectoryServer thread run-------------------------";
    return ((TrajectoryServer*)classPtr)->threadRun(NULL);
}

void TrajectoryServer::createThread()
{
    int ret=pthread_create( &threadId, NULL, &TrajectoryServer::threadTask,this);
    motionDebugInfo.threadId=syscall(SYS_gettid);
    qDebug()<<"TrajectoryServer createThread-------------------------threadId"<<motionDebugInfo.threadId;

}



PingPongBuffer* TrajectoryServer::getPingPongBuffer()
{

    return pingPongBuffer;
}

int TrajectoryServer::recordDataToWaveBuffer()//耗时约50us
{
    motionDebugInfo.realThreadStage=E_REAL_THREAD_RECORD_WAVE;
    PositionInfo tmpVehiclePositionFeedback;
    E_VEHICLE_POSITION_TYPE tmpType;
    vehiclePositionManage->getVehiclePosition_default(tmpVehiclePositionFeedback,tmpType);
    QVector<double> idealtorqueOut;
    getIdealTorque(idealtorqueOut);
    pthread_mutex_lock(&trajMutex);//test//不加锁时coredump,估计只读没问题，读写就必须加锁
    timeStamp++;
    int writeIndex=pingPongBuffer->currentWriteBufferIndex;
    int writeSubIndex=pingPongBuffer->currentWriteBufferSubIndex;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].timeStamp =timeStamp;
    for (int i = 0; i < jointsNum; i++)
    {
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].position_command_puls[i] =
                (*axisGroup->axisGroupDecouple->motorDataCommandVector)[i].position;
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].velocity_command_puls[i] =
                (*axisGroup->axisGroupDecouple->motorDataCommandVector)[i].speed;
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].torque_command_puls[i] =
                (*axisGroup->axisGroupDecouple->motorDataCommandVector)[i].torque;
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].position_feedback_puls[i] =
                (*axisGroup->axisGroupDecouple->motorDataFeedbackVector)[i].position;
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].velocity_feedback_puls[i] =
                (*axisGroup->axisGroupDecouple->motorDataFeedbackVector)[i].speed;
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].torque_feedback_puls[i] =
                (*axisGroup->axisGroupDecouple->motorDataFeedbackVector)[i].torque;
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].position_followOffset_puls[i] =
                (*axisGroup->axisGroupDecouple->motorDataFeedbackVector)[i].position-(*axisGroup->axisGroupDecouple->motorDataCommandVector)[i].position;
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].velocity_followOffset_puls[i] =
                (*axisGroup->axisGroupDecouple->motorDataFeedbackVector)[i].speed-(*axisGroup->axisGroupDecouple->motorDataCommandVector)[i].speed;
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].torque_followOffset_puls[i] =
                (*axisGroup->axisGroupDecouple->motorDataFeedbackVector)[i].torque-(*axisGroup->axisGroupDecouple->motorDataCommandVector)[i].torque;
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].velocity_offset_command_puls[i] =
                (*axisGroup->axisGroupDecouple->motorDataCommandVector)[i].velocityOffset;
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].torque_offset_command_puls[i] =
                (*axisGroup->axisGroupDecouple->motorDataCommandVector)[i].torqueOffset;

        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].position_command[i] =
                axisGroup->axisGroupDecouple->positionCommand[i];
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].velocity_command[i] =
                axisGroup->axisGroupDecouple->velCommand[i];
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].torque_command[i] =
                axisGroup->axisGroupDecouple->torqueCommand[i];
        if(i<idealtorqueOut.size())
        {
            pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].idealTorque[i] =idealtorqueOut[i];
            pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].torqueBias[i]=idealtorqueOut[i]
                                                -axisGroup->axisGroupDecouple->torqueFeedbackDecoupled[i];
        }

        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].position_feedback[i] =
                axisGroup->axisGroupDecouple->positionFeedbackDecoupled[i];
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].velocity_feedback[i] =
                axisGroup->axisGroupDecouple->velFeedbackDecoupled[i];
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].torque_feedback[i] =
                axisGroup->axisGroupDecouple->torqueFeedbackDecoupled[i];
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].position_followOffset[i] =
                axisGroup->axisGroupDecouple->positionFeedbackDecoupled[i]-axisGroup->axisGroupDecouple->positionCommand[i];//不加锁时coredump,估计只读没问题，读写就必须加锁
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].velocity_followOffset[i] =
                axisGroup->axisGroupDecouple->velFeedbackDecoupled[i]-axisGroup->axisGroupDecouple->velCommand[i];
        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].torque_followOffset[i] =
                axisGroup->axisGroupDecouple->torqueFeedbackDecoupled[i]-axisGroup->axisGroupDecouple->torqueCommand[i];
//        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].velocity_offset_command[writeSubIndex] =
//                axisGroup->axisGroupDecouple->velOffsetCommand[i];
//        pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].torque_offset_command[writeSubIndex] =
//                axisGroup->axisGroupDecouple->torqueOffsetCommand[i];
    }
    VehiclePosition vehiclePositionOut;
    virtualLineControl->getVehiclePositionCommand(vehiclePositionOut);

    BiasControlInfo controlInfoOut;
    virtualLineControl->getBias(controlInfoOut);
    double deltaDistanceXOut=controlInfoOut.deltaDistanceX;
    double deltaDistanceYOut=controlInfoOut.deltaDistanceY;
    double deltaAngleOut=controlInfoOut.deltaAngle;
//    double absoluteDistanceOut=controlInfoOut.absoluteDistance;
//    double absoluteAngleOut=controlInfoOut.absoluteAngle;
    double absoluteDistanceOut=controlInfoOut.distanceFollowError;
    double absoluteAngleOut=controlInfoOut.angleFollowError;

    double anglePidOut;
    double xPidOut,yPidOut;
    magneticTrack->getPidOutput(anglePidOut,xPidOut,yPidOut);

    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].calculateBiasType=
            controlInfoOut.calculateBiasType;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].vehiclePositionCommand[0] =
            vehiclePositionOut.x;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].vehiclePositionCommand[1] =
            vehiclePositionOut.y;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].vehiclePositionCommand[2]=
            vehiclePositionOut.rotate;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].vehiclePositionFeedback[0]=
            tmpVehiclePositionFeedback.position.x;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].vehiclePositionFeedback[1]=
            tmpVehiclePositionFeedback.position.y;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].vehiclePositionFeedback[2]=
            tmpVehiclePositionFeedback.position.rotate;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].deltaDistance_x =
            deltaDistanceXOut;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].deltaDistance_y=
            deltaDistanceYOut;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].deltaAngle=
            deltaAngleOut;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].absoluteDistance=
            absoluteDistanceOut;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].absoluteAngle=
            absoluteAngleOut;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].anglePidOutput=
            anglePidOut;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].xPidOutput=
            xPidOut;
    pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].yPidOutput=
            yPidOut;
    //aDin
    for (int i = 0; i < 10; i++)
    {
        if(D_RECORD_ADIN)
        {
            pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].aDin[i] =ioController->getAi(i);
        }
        else
        {
            pingPongBuffer->buffer[writeIndex].mathData[writeSubIndex].aDin[i] =g_waveMonitorData[i];

        }

    }


    pingPongBuffer->currentWriteBufferSubIndex += 1;
    if (pingPongBuffer->currentWriteBufferSubIndex  >= pingPongBuffer->bufferValidateDataLength)
    {
        pingPongBuffer->currentWriteBufferSubIndex = 0;
        if (0 == pingPongBuffer->currentWriteBufferIndex)
        {
            pingPongBuffer->currentWriteBufferIndex = 1;
        }
        else
        {
            pingPongBuffer->currentWriteBufferIndex = 0;
        }
    }
    pthread_mutex_unlock(&trajMutex);//test
    return 1;
}


 int TrajectoryServer::printInformation(MoveParameter moveParameter, QString title)
 {
     if(0==debugFlag)
     {
         return 0;
     }
     qDebug() <<"-------------------------------------"<< title <<"------------------------------";
     qDebug() << "******"<<"===acc=="<<moveParameter.acc
                         <<"===cntRatio=="<<moveParameter.cntRatio
                         <<"==index==="<<moveParameter.index
                         <<"==lineNumber==="<<moveParameter.lineNumber
                         <<"==moveType==="<<moveParameter.moveType
                         <<"==pointType==="<<moveParameter.pointType
                         <<"===velocityUnit=="<<moveParameter.velocityUnit
                         <<"===velocity=="<<moveParameter.velocity
                         <<"==configString==="<<QString::fromStdString(moveParameter.targetPoint.configString)
                         <<"==groupNum==="<<moveParameter.targetPoint.groupNum
                         <<"==jointFlag==="<<moveParameter.targetPoint.jointFlag
                         <<"==pointNum==="<<moveParameter.targetPoint.pointNum
                         <<"==ufValue==="<<moveParameter.targetPoint.ufValue
                         <<"==utValue==="<<moveParameter.targetPoint.utValue << endl;
                         for(int j = 0; j < moveParameter.targetPoint.configInt.size();j++)
                         {
                             qDebug() <<"==configInt==="<<moveParameter.targetPoint.configInt[j];
                         }
                         for(int j = 0; j < moveParameter.targetPoint.extPositionValue.size();j++)
                         {
                             qDebug() <<"==extPositionValue==="<<moveParameter.targetPoint.extPositionValue[j];
                         }
                         for(int j = 0; j < moveParameter.targetPoint.positionValue.size();j++)
                         {
                             qDebug() <<"==positionValue==="<<moveParameter.targetPoint.positionValue[j];
                         }

        return 1;

 }

 int TrajectoryServer::printInformation(QVector<MoveParameter> moveParameterArry, QString title)
 {
     if(0==debugFlag)
     {
         return 0;
     }
     qDebug() <<"-------------------------------------"<< title <<"------------------------------";
     int size=moveParameterArry.size();
     for(int i=0;i<size;i++)
     {
         qDebug() << "/////////////"<<"command index"<<i;
         qDebug() << "******"<<"===acc=="<<moveParameterArry[i].acc
                             <<"===cntRatio=="<<moveParameterArry[i].cntRatio
                             <<"==index==="<<moveParameterArry[i].index
                             <<"==lineNumber==="<<moveParameterArry[i].lineNumber
                             <<"==moveType==="<<moveParameterArry[i].moveType
                             <<"==pointType==="<<moveParameterArry[i].pointType
                             <<"===velocityUnit=="<<moveParameterArry[i].velocityUnit
                             <<"===velocity=="<<moveParameterArry[i].velocity
                             <<"==configString==="<<QString::fromStdString(moveParameterArry[i].targetPoint.configString)
                             <<"==groupNum==="<<moveParameterArry[i].targetPoint.groupNum
                             <<"==jointFlag==="<<moveParameterArry[i].targetPoint.jointFlag
                             <<"==pointNum==="<<moveParameterArry[i].targetPoint.pointNum
                             <<"==ufValue==="<<moveParameterArry[i].targetPoint.ufValue
                             <<"==utValue==="<<moveParameterArry[i].targetPoint.utValue << endl;
                             for(int j = 0; j < moveParameterArry[i].targetPoint.configInt.size();j++)
                             {
                                 qDebug() <<"==configInt==="<<moveParameterArry[i].targetPoint.configInt[j];
                             }
                             for(int j = 0; j < moveParameterArry[i].targetPoint.extPositionValue.size();j++)
                             {
                                 qDebug() <<"==extPositionValue==="<<moveParameterArry[i].targetPoint.extPositionValue[j];
                             }
                             for(int j = 0; j < moveParameterArry[i].targetPoint.positionValue.size();j++)
                             {
                                 qDebug() <<"==positionValue==="<<moveParameterArry[i].targetPoint.positionValue[j];
                             }
     }

     return 1;
 }



 bool TrajectoryServer::isAxisZeroOk()
 {

     pthread_mutex_lock(&trajMutex);//test
     bool  flag=axisGroup->isAxisZeroOk();
     pthread_mutex_unlock(&trajMutex);//test
     return flag;


 }

 bool TrajectoryServer::isAxisZeroOk(int axisId)
 {
     pthread_mutex_lock(&trajMutex);//test
     bool  flag=axisGroup->isAxisZeroOk(axisId);
     pthread_mutex_unlock(&trajMutex);//test
     return flag;



 }

 int TrajectoryServer::getAxisZeroState(int axisId)
 {
     pthread_mutex_lock(&trajMutex);//test
     int  flag=axisGroup->getAxisZeroState(axisId);
     pthread_mutex_unlock(&trajMutex);//test
     return flag;

 }

 QVector<int> TrajectoryServer::getAbsoluteZeroMotorValue()
 {
     pthread_mutex_lock(&trajMutex);//test
     QVector<int>   flag=axisGroup->getAbsoluteZeroMotorValue();
     pthread_mutex_unlock(&trajMutex);//test
     return flag;
 }

 QVector<double> TrajectoryServer::getJointMechanicalZeroValue()
 {
     pthread_mutex_lock(&trajMutex);//test
     QVector<double>  flag=axisGroup->getJointMechanicalZeroValue();
     pthread_mutex_unlock(&trajMutex);//test
     return flag;
 }

 int TrajectoryServer::axisLosePower()
 {
     pthread_mutex_lock(&trajMutex);//test
     int  flag=axisGroup->axisLosePower();
     pthread_mutex_unlock(&trajMutex);//test
     return flag;

 }

 int TrajectoryServer::axisLosePower(int axisId)
 {
     pthread_mutex_lock(&trajMutex);//test
     int  flag=axisGroup->axisLosePower(axisId);
     pthread_mutex_unlock(&trajMutex);//test
     return flag;


 }

 int TrajectoryServer::calibrateRobotByOffset(std::vector<double> offsetIn)
 {
     if(  eMasterOP!=getMasterState())
     {
         if(debugFlag)
         {
             qDebug()<< "calibrateRobot() error, eMasterOP!=getMasterState";
         }

         addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5025, robotId);
         resetAll(108);
         return -1;
     }
     if(  ENUM_TRAJECTORY_STOPPED!=basicStatus)
     {
         if(debugFlag)
         {
             qDebug()<< "calibrateRobot() error, ENUM_TRAJECTORY_STOPPED!=basicStatus";
         }

         addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 5034, robotId);
         return -2;
     }
     isMotorCommandValid=false;
     pthread_mutex_lock(&trajMutex);//test
     int  flag=axisGroup->calibrateRobotByOffset(offsetIn);
     calibrateReset();
     pthread_mutex_unlock(&trajMutex);//test
     servoOff_all();
     synchronizeMotorFeedbackToCalculatePosition();
     sleep(1);
     isMotorCommandValid=true;
     return flag;

 }

 int TrajectoryServer::validateZeroData()
 {
     pthread_mutex_lock(&trajMutex);//test
     int  flag=axisGroup->validateZeroData();
     pthread_mutex_unlock(&trajMutex);//test
     return flag;

 }

 int TrajectoryServer::calibrateRobot()
 {

     if(  eMasterOP!=getMasterState())
     {
         if(debugFlag)
         {
             qDebug()<< "calibrateRobot() error, eMasterOP!=getMasterState";
         }

         addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5025, robotId);
         resetAll(109);
         return -1;
     }
     if(  ENUM_TRAJECTORY_RUN==basicStatus)//报错时一直为暂停状态．
     {
         if(debugFlag)
         {
             qDebug()<< "calibrateRobot() error, ENUM_TRAJECTORY_STOPPED!=basicStatus";
         }

         addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 5034, robotId);
         return -2;
     }
     resetAll(110);
     isMotorCommandValid=false;
     pthread_mutex_lock(&trajMutex);//test
     int  flag=axisGroup->calibrateRobot();
     calibrateReset();
     pthread_mutex_unlock(&trajMutex);//test
     servoOff_all();
     synchronizeMotorFeedbackToCalculatePosition();
     sleep(1);
     isMotorCommandValid=true;
     return flag;



 }

 int TrajectoryServer::calibrateRobot(int axisId)
 {

     if(  eMasterOP!=getMasterState())
     {
         if(debugFlag)
         {
             qDebug()<< "calibrateRobot() error, eMasterOP!=getMasterState";
         }

         addMsg(ENUM_MSG_ERROR, "MotionServer", "TrajectoryServer", 5025, robotId);
         resetAll(111);
         return -1;
     }
     if(  ENUM_TRAJECTORY_RUN==basicStatus)//报错时一直为暂停状态．
     {
         if(debugFlag)
         {
             qDebug()<< "calibrateRobot() error, ENUM_TRAJECTORY_STOPPED!=basicStatus";
         }

         addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 5034, robotId);
         return -2;
     }
     resetAll(113);//报错时一直为暂停状态．
     isMotorCommandValid=false;
     pthread_mutex_lock(&trajMutex);//test
     int  flag=axisGroup->calibrateRobot(axisId);
     calibrateReset();
     pthread_mutex_unlock(&trajMutex);//test
     servoOff_all();
     synchronizeMotorFeedbackToCalculatePosition();
     sleep(1);
     isMotorCommandValid=true;
     return flag;
 }

 int TrajectoryServer::calibrateReset()
 {
     setMotorFeedbackDataVectorFromEthercatNoLock();
      //resetAll
     commandQueue.clear();
     axisGroup->resetAll();

     setBasicStatus(ENUM_TRAJECTORY_STOPPED,9990016);
     //resetTrajTime
     lineQueue.clear();
     timeDurationQueue.clear();
     currentLine=ENUM_LINE_INITIAL;
     isCurrentLineFinished=false;
     currentLineRemainingTime=-1;
     currentLineTimeDuration=-1;
     lineTimeCount=0;
     lineTimeCountOld=-1;

     allowSingleAxisJogFlag=false;

     return 1;
 }

 int TrajectoryServer::getCalibrateInformation(S_AXISGROUP_ZERO_INFORMATION &zeroInformationOut)
 {
     if(  ENUM_TRAJECTORY_STOPPED!=basicStatus)
     {
         if(debugFlag)
         {
             qDebug()<< "calibrateRobot() error, ENUM_TRAJECTORY_STOPPED!=basicStatus";
         }

         addMsg(ENUM_MSG_WARNING, "MotionServer", "TrajectoryServer", 5034, robotId);
         return -2;
     }
     pthread_mutex_lock(&trajMutex);//test
     int keyFlag=axisGroup->getCalibrateInformation(zeroInformationOut);
     pthread_mutex_unlock(&trajMutex);//test
     return keyFlag;
 }


 void TrajectoryServer::addMsg(int messageLevel, std::string componentName, std::string messageType
                               , int messageCode, int robotIdIn
                               , int parameter1, int parameter2, int parameter3, int parameter4)
 {
     Message tmpMsg;
     tmpMsg.MessageLevel = messageLevel;
     tmpMsg.componentClassName = componentName;
     tmpMsg.messageCode = messageCode;
     tmpMsg.messageType = messageType;
     tmpMsg.robotId = robotIdIn;

     string infomationStr;
     QString tmpStr;

     switch(messageCode)
     {
     case 5092:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("轴组寻零位开始").toStdString();
        infomationStr = str1;
        break;
     }
     case 5091:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("单轴(%1)寻零位开始").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
     }
     case 5090:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("连续插补命令圆弧段求解圆心失败").toStdString();
        infomationStr = str1;
        break;
     }
     case 5089:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("连续插补命令圆弧段参数错误").toStdString();
        infomationStr = str1;
        break;
     }
     case 5088:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("圆弧规划第二段非圆弧类型").toStdString();
        infomationStr = str1;
        break;
     }
     case 5087:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("圆弧规划缺少终止点").toStdString();
        infomationStr = str1;
        break;
     }
     case 5086:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("TP位置逆解失败").toStdString();
        infomationStr = str1;
        break;
     }
     case 5085:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("连续插补命令错误,轨迹类型(%1)不支持").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
     }
     case 5084:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("连续插补命令错误,点格式不为笛卡尔").toStdString();
        infomationStr = str1;
        break;
     }
     case 5083:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("连续插补命令参数错误").toStdString();
        infomationStr = str1;
        break;
     }
     case 5082:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("quickstop快速停止触发").toStdString();
        infomationStr = str1;
        break;
     }
     case 5081:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",碰撞检测触发：第(%1)轴超过合理力矩！").arg(parameter1);
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5080:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("拖动示教模式：禁止运行轨迹").toStdString();
        infomationStr = str1;
        break;
     }
     case 5079:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("dragConfig.xml配置文件加载错误").toStdString();
        infomationStr = str1;
        break;
     }
     case 5078:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",转换为笛卡尔点失败-圆弧终止点！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5077:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",转换为笛卡尔点失败-圆弧中间点！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5076:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",转换为笛卡尔点失败-直线目标点！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5075:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",笛卡尔运动自动限制关节速度-解除！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5074:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",笛卡尔运动自动限制关节速度-触发！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5073:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",当前位于奇异点附近，无法进行笛卡尔运动！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5072:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",当前位于奇异点附近，无法进行笛卡尔运动！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5071:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",获取跟踪数据失败！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5070:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",VMARK定位：位置精度恢复，继续运行！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5069:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",VMARK定位：位置(角度)误差过大，暂停！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5068:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",请停止机器人后，再修改参数！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5067:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",请停止机器人后，再获取参数！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5066:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",启动电机使能失败，主站超时未准备好！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5065:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",机械臂轨迹剩余时间无穷大错误！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5064:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",轴零点未校准，不允许使能！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5063:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",模拟量传感器获取数据失败，无法校准轴的零位！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5062:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",反光标记：位置精度恢复，继续运行！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5061:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",反光标记：位置(角度)误差过大，暂停！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5060:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",轨迹计算失败！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5059:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",转换为笛卡尔点失败！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5058:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",轴安全检查触发急停！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5057:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",转换为关节点失败！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5056:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",非停止状态，获取用户坐标系和工具坐标系信息失败！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5055:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",点数据为空，转换笛卡尔坐标失败！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5054:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",轴号(%1)超过范围！").arg(parameter1);
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5053:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",坐标点转换为关节点失败！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5052:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",监测到激光雷达数据恢复！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5051:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",监测到激光雷达数据异常中断，暂停了轨迹运行！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5050:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",qucodeSensorConfig.xml参数读取失败！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5049:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",cu1传感器socket连接失败！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5048:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",cu1传感器数据通讯失败次数超过允许值！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5047:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",cu1SensorConfig.xml参数读取失败！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5046:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",跟线策略设置为全向失败，因为当前车机构类型不支持！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5045:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",agv类型参数读取失败！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5044:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",请注意：正在创建地图中，不允许运行运动指令！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5043:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",没有可以回零的轴！");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5042:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",轴%1切换到回零模式错误，zeroConfig.xml配置文件不允许回零操作").arg(parameter1+1);
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5041:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",切换到回零模式错误，zeroConfig.xml配置文件不允许回零操作");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5040:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",轴组回零完成");
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5039:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",轴(%1)回零完成").arg(parameter1+1);
         infomationStr = tmpStr.toStdString();
         break;
     }
     case 5038:
     {
         tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
         tmpStr = tmpStr + QObject::tr(",轴(%1)回零时发生错误").arg(parameter1);
         infomationStr = tmpStr.toStdString();
         break;
     }

     case 5037:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("没有进行机器人零点校准，可以关节手动，注意超过机械限位（系统此时没有限位保护）!").toStdString();
        infomationStr = str1;
        break;
     }
     case 5036:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("电机使能失败，当前算法第(%1)轴命令值与反馈值偏差(%2)超过允许设定值(%3)")
                .arg(parameter1).arg(parameter2).arg(allowServonCheckDifference).toStdString();
        infomationStr = str1;
        break;
     }
     case 5035:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("指令点数据异常，程序文件轴个数与实际机器人轴数(%1)不一致！").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
     }
     case 5034:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("机器人零点校准失败，机器人正在运行中！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5033:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("非servo on状态，无法执行运动指令！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5032:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("轮速度 转换 车速 失败！").toStdString();
        infomationStr = str1;
        break;
     }


     case 5029:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("车体手动控制模块启动失败！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5028:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("磁条传感器没有感应到磁条！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5027:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("磁条跟踪模块启动失败,请检查配置参数是否完整").toStdString();
        infomationStr = str1;
        break;
     }
     case 5026:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("机器人坐标系初始化失败，无法运行，可以关节手动！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5025:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("机器人零点校准失败，总线没有进入ＯＰ模式！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5024:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("机器人没有使能，不允许运行指令程序！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5023:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("机器人零点没有校准，不允许运行指令程序！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5022:
     {

        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("直线平滑指令处理失败错误！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5021:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("运动指令点执行失败，旧轨迹没有执行完！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5020:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("运动指令点的多解信息与当前位置不匹配，请手动调整当前机器人位置或修改运动指令点！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5019:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("运动指令点的坐标系号与当前使用的坐标系号不一致！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5018:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("直线指令处理失败错误，可能算法忙！").toStdString();
        infomationStr = str1;
        break;
     }
     case 5017:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("圆弧指令缺少第二点数据").toStdString();
        infomationStr = str1;
        break;
     }
     case 5016:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("目标点超过时间%1毫秒未到达目标点允许误差范围！").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
     }

     case 5014:
     {
        tmpStr = QObject::tr("机器人(ID:%1)关节数量冲突错误，算法轴数＝%2,机器人轴数＝%3 .")
                .arg(robotIdIn).arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
     }
         case 5013:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")").toStdString() +
                    QObject::tr("运动指令JOINT_FLAG不支持！").toStdString();
            infomationStr = str1;
            break;
         }
         case 5012:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")").toStdString() +
                    QObject::tr("运动指令单位不支持！").toStdString();
            infomationStr = str1;
            break;
         }
     case 5006:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("轴解偶错误").toStdString();
        infomationStr = str1;
        break;
     }
     case 5005:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("轴运动错误").toStdString();
        infomationStr = str1;
        break;
     }
     case 5004:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("轨迹运动错误").toStdString();
        infomationStr = str1;
        break;
     }



         default:
         {
             qDebug()<<"Coordinate::addMsg====not match error code";
             break;
         }
     }

     tmpMsg.MessageInformation = infomationStr;

     #ifndef MOTION_SERVER_UNIT_TEST
     motionMessage->addMsg(tmpMsg);
     #endif
 }
